# 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
      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:
            def minDistFn(d,value,x):
                tempp = x[:]
                tempp[d] = value

                #HACK: for pendulum
                if SO2_HACK and d==0:
                    tempp2 = x[:]
                    tempp2[d] = math.pi+value
                    return min(self.metric(tempp,x),self.metric(tempp2,x))
                #HACK: for SE2
                if SE2_HACK and d==2:
                    tempp2 = x[:]
                    tempp2[d] = math.pi+value
                    return min(self.metric(tempp,x),self.metric(tempp2,x))
                return self.metric(tempp,x)
            def partitionFn(d,value,x):
                #HACK: for pendulum
                if SO2_HACK and d == 0:
                    math.copysign(1,so2.diff(x[d],value))
                #HACK: for SE2
                if SE2_HACK and d == 2:

# 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

      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:
            def minDistFn(d,value,x):
                tempp = x[:]
                tempp[d] = value

                #HACK: for pendulum
                if SO2_HACK and d==0:
                    tempp2 = x[:]
                    tempp2[d] = math.pi+value
                    return min(self.metric(tempp,x),self.metric(tempp2,x))
                #HACK: for SE2
                if SE2_HACK and d==2:
                    tempp2 = x[:]
                    tempp2[d] = math.pi+value
                    return min(self.metric(tempp,x),self.metric(tempp2,x))
                return self.metric(tempp,x)
            def partitionFn(d,value,x):
                #HACK: for pendulum
                if SO2_HACK and d == 0:
                    math.copysign(1,so2.diff(x[d],value))
                #HACK: for SE2
                if SE2_HACK and d == 2:

                    math.copysign(1,so2.diff(x[d],value))