# Path: pomp/spaces/metric.py
# def euclideanMetric(a,b):
# def L2Metric(a,b):
# def L1Metric(a,b):
# def LinfMetric(a,b):
#     def __init__(self,w):
#     def __call__(self,a,b):
#     def __init__(self,p):
#     def __call__(self,a,b):
# class WeightedEuclideanMetric:
# class LpMetric:
# 
# Path: pomp/klampt/so2.py
# def identity():
# def inv(a):
# def apply(a,pt):
# def normalize(a):
# def diff(a,b):
# def interp(a,b,u):
# def compose(a,b):
# def matrix(a):
# def from_matrix(R):


from six import iteritems
from .knn import *
from ..spaces import metric
from ..klampt import so2
import math
        """
        assert isinstance(points,(list,tuple))
        self.points = points
        self.splitdim = splitdim
        self.depth = 0
        self.splitvalue = None
        self.left = None
        self.right = None

class KDTree:
    """
    Attributes:
    - maxPointsPerNode: allows up to this number of points in a single K-D tree
      node.
    - partitionFn: a function (d,value,x) that returns -1,0, or 1
      depending on whether the point is on the positive, negative, or on the
      plane with split value value on the d'th dimension.  By default, it's
      sign(x[d] - value).
    - minDistanceFn: a function (d,value,x) that returns the minimum
      distance to the partition plane on the d'th dimension.  By default, it's
      |x[d] - value| for the euclidean metric, and otherwise it's
      metric(set(x,d,value),x[d]) where set(x,d,value) is a point equal
      to x except for it's d'th entry set to value.  This latter case works for
      all L-p norms, weighted or unweighted.
    """
    def __init__(self,distanceMetric=metric.euclideanMetric):
        self.root = None
        self.metric = distanceMetric
        self.partitionFn = lambda d,value,x: math.copysign(1,x[d]-value)
        self.minDistanceFn = lambda d,value,x: abs(x[d]-value)

# Path: pomp/spaces/metric.py
# def euclideanMetric(a,b):
# def L2Metric(a,b):
# def L1Metric(a,b):
# def LinfMetric(a,b):
#     def __init__(self,w):
#     def __call__(self,a,b):
#     def __init__(self,p):
#     def __call__(self,a,b):
# class WeightedEuclideanMetric:
# class LpMetric:
# 
# Path: pomp/klampt/so2.py
# def identity():
# def inv(a):
# def apply(a,pt):
# def normalize(a):
# def diff(a,b):
# def interp(a,b,u):
# def compose(a,b):
# def matrix(a):
# def from_matrix(R):


# Path: pomp/structures/kdtree.py
from six import iteritems
from .knn import *
from ..spaces import metric
from ..klampt import so2
import math

        """
        assert isinstance(points,(list,tuple))
        self.points = points
        self.splitdim = splitdim
        self.depth = 0
        self.splitvalue = None
        self.left = None
        self.right = None

class KDTree:
    """
    Attributes:
    - maxPointsPerNode: allows up to this number of points in a single K-D tree
      node.
    - partitionFn: a function (d,value,x) that returns -1,0, or 1
      depending on whether the point is on the positive, negative, or on the
      plane with split value value on the d'th dimension.  By default, it's
      sign(x[d] - value).
    - minDistanceFn: a function (d,value,x) that returns the minimum
      distance to the partition plane on the d'th dimension.  By default, it's
      |x[d] - value| for the euclidean metric, and otherwise it's
      metric(set(x,d,value),x[d]) where set(x,d,value) is a point equal
      to x except for it's d'th entry set to value.  This latter case works for
      all L-p norms, weighted or unweighted.
    """
    def __init__(self,distanceMetric=metric.euclideanMetric):
        self.root = None
        self.metric = distanceMetric
        self.partitionFn = lambda d,value,x: math.copysign(1,x[d]-value)
        self.minDistanceFn = lambda d,value,x: abs(x[d]-value)

        if self.metric != metric.euclideanMetric: