Subversion Repositories mkgmap

/*
 * Copyright (C) 2014 Gerd Petermann
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 3 or
 * version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 */
package uk.me.parabola.util;


import java.util.LinkedHashSet;
import java.util.Set;
import uk.me.parabola.imgfmt.app.Coord;


/**
 * A kd-tree (2D) implementation to solve the nearest neighbor problem.
 * The tree is not explicitly balanced.
 *
 * @author Gerd Petermann
 *
 */
public class KdTree <T extends Locatable> {
        private static final boolean ROOT_NODE_USES_LONGITUDE = false;
       
        private class KdNode {
                T point;
                KdNode left;
                KdNode right;

                KdNode(T p) {
                        point = p;
                }
        }
        // the tree root
    private KdNode root;
    // number of saved objects  
    private int size;

    // helpers
    private T nextPoint ;
    private double minDist;
    private double maxDist;
    private Set<T> set;

    /**
     *  create an empty tree
     */
        public KdTree() {
                root = null;
        }

        public long size()
        {
                return size;
        }

       
        /**
         * Start the add action with the root
         * @param toAdd
         */
        public void add(T toAdd) {
                size++;
                root = add(toAdd, root, ROOT_NODE_USES_LONGITUDE);
        }

        /**
         * Compares the given axis of both points.
         * @param longitude <code>true</code>: compare longitude; <code>false</code> compare latitude
         * @param c1 a point
         * @param c2 another point
         * @return <code>true</code> the axis value of c1 is smaller than c2;
         *              <code>false</code> the axis value of c1 is equal or larger than c2
         */
        private boolean isSmaller(boolean longitude, Coord c1, Coord c2) {
                if (longitude) {
                        return c1.getLongitude() < c2.getLongitude();
                } else {
                        return c1.getLatitude() < c2.getLatitude();
                }
        }
       
        /**
         * Recursive routine to find the right place for inserting
         * into the tree.  
         * @param toAdd the point
         * @param tree the subtree root node where to add (maybe <code>null</code>)
         * @param useLongitude <code>true</code> the tree node uses longitude for comparison;
         *              <code>false</code> the tree node uses latitude for comparison
         * @return the subtree root node after insertion
         */
    private KdNode add( T toAdd, KdNode tree,  boolean useLongitude){
        if( tree == null ) {
            tree = new KdNode( toAdd );
        } else {
                if(isSmaller(useLongitude, toAdd.getLocation(), tree.point.getLocation())) {
                        tree.left = add(toAdd, tree.left, !useLongitude);
                } else {
                        tree.right = add(toAdd, tree.right, !useLongitude);
                }
        }
        return tree;
    }
   
        /**
         * Searches for the point that has smallest distance to the given point.
         * @param p the point to search for
         * @return the point with shortest distance to <var>p</var>
         */
        public T findNextPoint(Locatable p) {
                // reset
                minDist = Double.MAX_VALUE;
                maxDist = -1;
                set = null;
                nextPoint = null;
               
                // false => first node is a latitude level
                return findNextPoint(p, root, ROOT_NODE_USES_LONGITUDE);
        }

        /**
         * Searches for the point that has smallest distance to the given point.
         * @param p the point to search for
         * @return the point with shortest distance to <var>p</var>
         */
        public Set<T> findNextPoint(Locatable p, double maxDist) {
                // reset
                minDist = Double.MAX_VALUE;
                this.maxDist = Math.pow(maxDist * 360 / Coord.U, 2); // convert maxDist in meter to distanceInDegreesSquared
                nextPoint = null;
                this.set = new LinkedHashSet<>();
                // false => first node is a latitude level
                findNextPoint(p, root, ROOT_NODE_USES_LONGITUDE);
                return set;
        }

        private T findNextPoint(Locatable p, KdNode tree, boolean useLongitude) {
                boolean continueWithLeft = false;
                if (tree == null)
                        return nextPoint;
               
                if (tree.left == null && tree.right == null){
                        double dist = tree.point.getLocation().distanceInDegreesSquared(p.getLocation());
                        if (dist <= maxDist && set != null){
                                set.add(tree.point);
                        }
                        if (dist < minDist){
                                nextPoint = tree.point;
                                if (dist < maxDist)
                                        minDist = maxDist;
                                else
                                        minDist = dist;
                        }
                        return nextPoint;
                }
                else {
                        if (isSmaller(useLongitude, p.getLocation(), tree.point.getLocation())){
                                continueWithLeft = false;
                                nextPoint = findNextPoint(p, tree.left, !useLongitude);
                        }
                        else {
                                continueWithLeft = true;
                                nextPoint = findNextPoint(p, tree.right, !useLongitude);
                        }
                }
               
                double dist = tree.point.getLocation().distanceInDegreesSquared(p.getLocation());
                if (dist <= maxDist && set != null)
                        set.add(tree.point);
                if (dist < minDist){
                        nextPoint = tree.point;
                        minDist = dist;
                        if (dist < maxDist)
                                minDist = maxDist;
                        else
                                minDist = dist;
                }              
                // do we have to search the other part of the tree?
                Coord test;
                if (useLongitude)
                        test = Coord.makeHighPrecCoord(p.getLocation().getHighPrecLat(), tree.point.getLocation().getHighPrecLon());
                else
                        test = Coord.makeHighPrecCoord(tree.point.getLocation().getHighPrecLat(), p.getLocation().getHighPrecLon());
                if (test.distanceInDegreesSquared(p.getLocation()) < minDist){
                        if (continueWithLeft)
                                nextPoint = findNextPoint(p, tree.left, !useLongitude);
                        else
                                nextPoint = findNextPoint(p, tree.right, !useLongitude);
                }
                return nextPoint;
        }
}