# Path: pomp/klampt/gldraw.py
# def point(p):
# def circle(center,radius,res=0.01,filled=True):
# def triangle(a,b,c,lighting=True,filled=True):
# def quad(a,b,c,d,lighting=True,filled=True):
# def box(a=(0,0,0),b=(1,1,1),lighting=True,filled=True):
# def centered_box(dims=(1,1,1),lighting=True,filled=True):
# def setcolor(r,g,b,a=1.0,lighting=True):
# def xform_widget(T,length,width,lighting=True,fancy=False):
# def glutBitmapString(font,string):
# 
# Path: pomp/spaces/configurationspace.py
# class NeighborhoodSubset(Set):
#     """A ball of radius r around a point c, with a distance given by
#     a ConfigurationSpace.
#     """
#     def __init__(self,space,c,r):
#         self.space = space
#         self.c = c
#         self.r = r
# 
#     def contains(self,x):
#         return self.space.distance(x,self.c) <= self.r
# 
#     def sample(self):
#         return self.space.sampleNeighborhood(self.c,self.r)
# 
#     def project(self,x):
#         d = self.space.distance(x,self.c)
#         if d <= self.r: return x
#         return self.space.interpolate(x,self.c,(d-self.r)/d)
# 
#     def signedDistance(self,x):
#         d = self.space.distance(x,self.c)
#         return d - self.r
# 
# class SingletonSubset(Set):
#     """A single point, with a distance given by a ConfigurationSpace.
#     """
#     def __init__(self,space,c):
#         self.space = space
#         self.c = c
# 
#     def contains(self,x):
#         return x == self.c
# 
#     def sample(self):
#         return self.c
# 
#     def project(self,x):
#         return self.c
# 
#     def signedDistance(self,x):
#         return self.space.distance(x,self.c)
# 
# Path: pomp/planners/problem.py
# class PlanningProblem:
#     def __init__(self,space,
#                  start=None,
#                  goal=None,
#                  objective=None,
#                  visualizer=None,
#                  heuristic=None,
#                  costLowerBound=None,
#                  goalRadius=None,
#                  euclidean=False):
#         self.space = space
#         if isinstance(space,ControlSpace):
#             self.controlSpace = space
#             self.configurationSpace = space.configurationSpace()
#         else:
#             self.controlSpace = None
#             self.configurationSpace = space
#         if goalRadius != None and isinstance(goal,(list,tuple)):
#             goal = NeighborhoodSubset(self.configurationSpace,goal,goalRadius)
#         self.start = start
#         self.goal = goal
#         self.objective = objective
#         self.visualizer = visualizer
#         self.heuristic = heuristic
#         self.costLowerBound = costLowerBound
#         self.euclidean = euclidean
# 
#     def cartesian(self):
#         return self.euclidean
#     def pointToPoint(self):
#         return isinstance(self.goal,SingletonSubset) or isinstance(goal,(list,tuple))
#     def differentiallyConstrained(self):
#         return self.controlSpace != None
#     def planner(self,type,**params):
#         d = metric.euclideanMetric if self.euclidean else self.configurationSpace.distance
#         return allplanners.makePlanner(type,space=self.space,
#                            start=self.start,goal=self.goal,
#                            objective=self.objective,
#                            heuristic=self.heuristic,
#                            metric=d,
#                            costLowerBound=self.costLowerBound,
#                            **params)


from OpenGL.GL import *
from ..klampt import gldraw
from ..spaces.objectives import *
from ..spaces.configurationspace import *
from ..spaces.edgechecker import *
from ..spaces.metric import *
from ..spaces.configurationspace import NeighborhoodSubset,SingletonSubset
from ..planners.problem import PlanningProblem

            res.append(o.signedDistance_gradient(x))
        return np.vstack(res)


def circleTest():
    space = Geometric2DCSpace()
    space.addObstacle(Circle(0.5,0.4,0.39))
    start=[0.06,0.25]
    goal=[0.94,0.25]
    objective = PathLengthObjectiveFunction()
    goalRadius = 0.1
    return PlanningProblem(space,start,goal,
                           objective=objective,
                           visualizer=space,
                           costLowerBound = vectorops.distance,
                           goalRadius = goalRadius,
                           euclidean = True)


def rrtChallengeTest():
    w = 0.03
    eps = 0.01
    space = Geometric2DCSpace()
    space.box = BoxSet([0,0],[1,w*2+eps])
    space.addObstacle(Box(0,w*2+eps,1,1))
    space.addObstacle(Box(w,w,1,w+eps))
    start=[1-w*0.5,w+eps+w*0.5]
    goal=[1-w*0.5,w*0.5]
    goalRadius = w*0.5
    objective = PathLengthObjectiveFunction()

# Path: pomp/klampt/gldraw.py
# def point(p):
# def circle(center,radius,res=0.01,filled=True):
# def triangle(a,b,c,lighting=True,filled=True):
# def quad(a,b,c,d,lighting=True,filled=True):
# def box(a=(0,0,0),b=(1,1,1),lighting=True,filled=True):
# def centered_box(dims=(1,1,1),lighting=True,filled=True):
# def setcolor(r,g,b,a=1.0,lighting=True):
# def xform_widget(T,length,width,lighting=True,fancy=False):
# def glutBitmapString(font,string):
# 
# Path: pomp/spaces/configurationspace.py
# class NeighborhoodSubset(Set):
#     """A ball of radius r around a point c, with a distance given by
#     a ConfigurationSpace.
#     """
#     def __init__(self,space,c,r):
#         self.space = space
#         self.c = c
#         self.r = r
# 
#     def contains(self,x):
#         return self.space.distance(x,self.c) <= self.r
# 
#     def sample(self):
#         return self.space.sampleNeighborhood(self.c,self.r)
# 
#     def project(self,x):
#         d = self.space.distance(x,self.c)
#         if d <= self.r: return x
#         return self.space.interpolate(x,self.c,(d-self.r)/d)
# 
#     def signedDistance(self,x):
#         d = self.space.distance(x,self.c)
#         return d - self.r
# 
# class SingletonSubset(Set):
#     """A single point, with a distance given by a ConfigurationSpace.
#     """
#     def __init__(self,space,c):
#         self.space = space
#         self.c = c
# 
#     def contains(self,x):
#         return x == self.c
# 
#     def sample(self):
#         return self.c
# 
#     def project(self,x):
#         return self.c
# 
#     def signedDistance(self,x):
#         return self.space.distance(x,self.c)
# 
# Path: pomp/planners/problem.py
# class PlanningProblem:
#     def __init__(self,space,
#                  start=None,
#                  goal=None,
#                  objective=None,
#                  visualizer=None,
#                  heuristic=None,
#                  costLowerBound=None,
#                  goalRadius=None,
#                  euclidean=False):
#         self.space = space
#         if isinstance(space,ControlSpace):
#             self.controlSpace = space
#             self.configurationSpace = space.configurationSpace()
#         else:
#             self.controlSpace = None
#             self.configurationSpace = space
#         if goalRadius != None and isinstance(goal,(list,tuple)):
#             goal = NeighborhoodSubset(self.configurationSpace,goal,goalRadius)
#         self.start = start
#         self.goal = goal
#         self.objective = objective
#         self.visualizer = visualizer
#         self.heuristic = heuristic
#         self.costLowerBound = costLowerBound
#         self.euclidean = euclidean
# 
#     def cartesian(self):
#         return self.euclidean
#     def pointToPoint(self):
#         return isinstance(self.goal,SingletonSubset) or isinstance(goal,(list,tuple))
#     def differentiallyConstrained(self):
#         return self.controlSpace != None
#     def planner(self,type,**params):
#         d = metric.euclideanMetric if self.euclidean else self.configurationSpace.distance
#         return allplanners.makePlanner(type,space=self.space,
#                            start=self.start,goal=self.goal,
#                            objective=self.objective,
#                            heuristic=self.heuristic,
#                            metric=d,
#                            costLowerBound=self.costLowerBound,
#                            **params)


# Path: pomp/example_problems/geometric.py
from OpenGL.GL import *
from ..klampt import gldraw
from ..spaces.objectives import *
from ..spaces.configurationspace import *
from ..spaces.edgechecker import *
from ..spaces.metric import *
from ..spaces.configurationspace import NeighborhoodSubset,SingletonSubset
from ..planners.problem import PlanningProblem

            res.append(o.signedDistance_gradient(x))
        return np.vstack(res)


def circleTest():
    space = Geometric2DCSpace()
    space.addObstacle(Circle(0.5,0.4,0.39))
    start=[0.06,0.25]
    goal=[0.94,0.25]
    objective = PathLengthObjectiveFunction()
    goalRadius = 0.1
    return PlanningProblem(space,start,goal,
                           objective=objective,
                           visualizer=space,
                           costLowerBound = vectorops.distance,
                           goalRadius = goalRadius,
                           euclidean = True)


def rrtChallengeTest():
    w = 0.03
    eps = 0.01
    space = Geometric2DCSpace()
    space.box = BoxSet([0,0],[1,w*2+eps])
    space.addObstacle(Box(0,w*2+eps,1,1))
    space.addObstacle(Box(w,w,1,w+eps))
    start=[1-w*0.5,w+eps+w*0.5]
    goal=[1-w*0.5,w*0.5]
    goalRadius = w*0.5
    objective = PathLengthObjectiveFunction()

    return PlanningProblem(space,start,goal,
