# Path: pomp/klampt/vectorops.py
# def add(*items):
# def madd(a,b,c):
# def sub(a,b):
# def mul(a,b):
# def div(a,b):
# def maximum(a,b):
# def minimum(a,b):
# def dot(a,b):
# def normSquared(a):
# def norm(a):
# def unit(a,epsilon=1e-5):
# def norm_L1(a):
# def norm_Linf(a):
# def distanceSquared(a,b):
# def distance(a,b):
# def cross(a,b):
# def interpolate(a,b,u):


from builtins import range
from six import iteritems
from . import differences
from . import sampling
from ..klampt import vectorops
import math
import random
import numpy as np
    def project(self,x):
        return self.x

    def signedDistance(self,x):
        d = np.asarray(x)-np.asarray(self.x)
        return np.dot(d,d)
    
    def signedDistance_gradient(self,x):
        d = np.asarray(x)-np.asarray(self.x)
        return 2*d


class NeighborhoodSet(Set):
    """A ball of radius r around a point c"""
    def __init__(self,c,r):
        self.c = c
        self.r = r
        
    def bounds(self):
        return [[v-self.r for v in self.c],[v+self.r for v in self.c]]

    def contains(self,x):
        return vectorops.distance(x,self.c) <= self.r

    def sample(self):
        return sampling.sample_hyperball(len(self.c),self.c,self.r)

    def project(self,x):
        d = vectorops.distance(x,self.c)
        if d <= self.r: return x

# Path: pomp/klampt/vectorops.py
# def add(*items):
# def madd(a,b,c):
# def sub(a,b):
# def mul(a,b):
# def div(a,b):
# def maximum(a,b):
# def minimum(a,b):
# def dot(a,b):
# def normSquared(a):
# def norm(a):
# def unit(a,epsilon=1e-5):
# def norm_L1(a):
# def norm_Linf(a):
# def distanceSquared(a,b):
# def distance(a,b):
# def cross(a,b):
# def interpolate(a,b,u):


# Path: pomp/spaces/sets.py
from builtins import range
from six import iteritems
from . import differences
from . import sampling
from ..klampt import vectorops
import math
import random
import numpy as np

    def project(self,x):
        return self.x

    def signedDistance(self,x):
        d = np.asarray(x)-np.asarray(self.x)
        return np.dot(d,d)
    
    def signedDistance_gradient(self,x):
        d = np.asarray(x)-np.asarray(self.x)
        return 2*d


class NeighborhoodSet(Set):
    """A ball of radius r around a point c"""
    def __init__(self,c,r):
        self.c = c
        self.r = r
        
    def bounds(self):
        return [[v-self.r for v in self.c],[v+self.r for v in self.c]]

    def contains(self,x):
        return vectorops.distance(x,self.c) <= self.r

    def sample(self):
        return sampling.sample_hyperball(len(self.c),self.c,self.r)

    def project(self,x):
        d = vectorops.distance(x,self.c)
        if d <= self.r: return x

        return vectorops.madd(x,vectorops.sub(self.c,x),(d-self.r)/d)