def bisection_pointwise0_6(iobj,
x1_arr_,
x2_arr_,
ROOT_TOLERANCE=ROOT_TOLERANCE):
# A point-wise version (version 1)
""" x1_arr_ must be outside and x2_arr_ must be inside the object.
Then this function finds points x=x1_arr+(lambda)*(x2_arr_-x1_arr_) where f(x)=0 using the bisection method."""
check_vector4_vectorized(x1_arr_)
check_vector4_vectorized(x2_arr_)
assert x1_arr_.shape[0] == x2_arr_.shape[0]
result_x_arr = np.zeros(x1_arr_.shape)
EPS = 0.000001 # sign
n = x1_arr_.shape[0]
for i in range(n):
x1v = x1_arr_[np.newaxis, i, :].copy()
x2v = x2_arr_[np.newaxis, i, :].copy()
v1_arr = iobj.implicitFunction(x1v)[0]
v2_arr = iobj.implicitFunction(x2v)[0]
MAX_ITER = 20
result_x_arr[i, :], iterations = bisection_3_standard(
iobj, x1v, x2v, v1_arr, v2_arr, MAX_ITER)
return result_x_arr
def ellipsoid_point_and_gradient_vectorized(self, m, xa, correctGrad, center=None):
""" Checks if the point x is on the surface, and if the gradient is correct."""
e = vectorized.Ellipsoid(m)
msg = "vectorized.Ellipsoid(m): "+str(e)
va = e.implicitFunction(xa)
ga = e.implicitGradient(xa)
check_vector4_vectorized(ga)
N = xa.shape[0]
check_scalar_vectorized(va, N)
assert ga.ndim == 2
assert ga.shape == (N, 4)
assert correctGrad.shape == (N, 4)
correctScalar = 0
less_a = np.less(np.abs(va - correctScalar), TOLERANCE)
#print(va - correctScalar)
if not np.all(less_a):
print("Some error:")
print(xa)
print(va)
print(ga)
print(e)
self.assertTrue(np.all(less_a), ("Implicit Function's scalar value incorrect"))
for i in range(ga.shape[0]):
(are_parallel,are_directed) = vectors_parallel_and_direction(ga[i, 0:3], correctGrad[i, 0:3])
self.assertTrue(are_parallel, "Incorrect gradient: not parallel "+msg)
self.assertTrue( are_directed, "parallel but opposite directions "+msg)
def project_single_point2_ohtake(iobj, start_x, lambda_val, max_dist):
#dont use this
""" lambda_val: step size"""
#max_iter = 20 # config["max_iter"]
check_vector4_vectorized(start_x)
assert start_x.shape[0] == 1
p1 = search_near_using_gradient_ohtake(iobj, start_x, None, lambda_val,
max_dist)
f1 = iobj.implicitFunction(p1)
p2 = 2 * start_x - p1
f2 = iobj.implicitFunction(p2)
if f1 * f2 < 0:
direction = -(start_x - p1) # as in Ohtake
dn = np.linalg.norm(direction)
if dn > 0.000000001:
direction = direction / dn
#broken
p2_ = search_near_using_vector_ohtake(iobj, start_x, direction,
lambdaa, max_dist)
if np.linalg.norm(start_x - p2_) > np.linalg.norm(start_x - p1):
p = p2_
else:
p = p1
else:
p = p1
if np.linalg.norm(start_x - p) <= max_dist:
return p
else:
return None
def test_ellipsoid_random_points(self):
"""Testing hundreds of random points on a sphere of size RADIUS"""
for i in range(0, 30):
RADIUS = 3
POW = 4 # higher POW will get points more toward parallel to axes
N = 500
#rcenter = make_random_vector_vectorized(N, 1000, 1.0)
rcenter = make_random_vector(1000, 1.0)
centers_a = repeat_vect4(N, rcenter)
r0 = make_random_vector_vectorized(N, RADIUS, POW)
r = r0 + centers_a
r[:, 3] = 1
assert r.shape[1] == 4 # generates vector4 , but the non vectorized version generates vector 3
xa = r # make_vector4( r[0], r[1], r[2] )
m = np.eye(4) * RADIUS
m[0:3, 3] = rcenter[0:3]
m[3, 3] = 1
expected_grad = -r0 # r - centers_a #repeat_vect4(N, make_vector4( r0[0], r0[1], r0[2] ) )
expected_grad[:,3] = 1
check_vector4_vectorized(expected_grad)
self.ellipsoid_point_and_gradient_vectorized(m, xa, expected_grad)
def implicitGradient(self, p):
check_vector4_vectorized(p)
sides = 6
na = np.zeros((sides, 4))
n = p.shape[0]
temp = np.zeros((n, sides))
for i in range(len(self.p0)):
p0 = self.p0[i]
n0 = self.n0[i]
n0[3] = 0
sub = p - np.tile(p0[np.newaxis, :], (n, 1))
assert np.allclose(sub[:, 3], 0)
#print("==================")
#print(sub.shape) #(1x4)
#print(n0.shape) #(4,)
vi = np.dot(sub, n0)
#print(vi)
temp[:, i] = vi
na[i, :] = n0
ia = np.argmin(temp, axis=1)
#va = np.amin(temp, axis=1)
assert ia.shape == (n, )
g = na[ia, :]
g[:, 3] = 1
check_vector4_vectorized(g)
return g
"""
def implicitFunction(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
c = 1.0-np.greater(va, vb)
v = va * c + vb * (1-c)
check_scalar_vectorized(v)
return v
def hessianMatrix(self, p):
check_vector4_vectorized(p)
ha = self.a.hessianMatrix(p)
hb = self.b.hessianMatrix(p)
#todo
h = ha * self.afactor + hb * self.bfactor
check_matrix3_vectorized(h)
return h
def implicitFunction(self, p):
check_vector4_vectorized(p)
tp = np.dot(self.invmatrix, np.transpose(p))
tp = np.transpose(tp)
v = self.base_object.implicitFunction(tp)
check_scalar_vectorized(v)
return v
def implicitFunction(self, pa):
check_vector4_vectorized(pa)
tp = np.dot(self.invmatrix, np.transpose(
pa)) # inefficient. todo: multiply from right => will be efficient
tp = np.transpose(tp) # inefficient.
v = self.sphere.implicitFunction(tp)
check_scalar_vectorized(v)
return v
def implicitFunction(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = -self.b.implicitFunction(p)
v = (1.0 / (1 + self.alpha)) * (
va + vb + self.sgn * np.sqrt(va**2 + vb**2 -
(2 * self.alpha) * va * vb))
check_scalar_vectorized(v)
return v
def numerical_gradient_slow_func(self_iobj, x):
""" Used as a slow implicitGradient(self, x) when too lasy to derive the gradient! see classes Screw, RSubtract"""
check_vector4_vectorized(x)
count = x.shape[0]
g = np.zeros((count, 4))
for i in range(x.shape[0]):
v = x[i, 0:4]
#inefficient: not vectorised
g[i, :] = numerical_gradient(self_iobj, v, is_vectorized=True)
return g
def bisection_pointwise1(iobj,
x1_arr_,
x2_arr_,
ROOT_TOLERANCE=ROOT_TOLERANCE):
# A point-wise version (version 1)
""" x1_arr_ must be outside and x2_arr_ must be inside the object.
Then this function finds points x=x1_arr+(lambda)*(x2_arr_-x1_arr_) where f(x)=0 using the bisection method."""
check_vector4_vectorized(x1_arr_)
check_vector4_vectorized(x2_arr_)
assert x1_arr_.shape[0] == x2_arr_.shape[0]
result_x_arr = np.zeros(x1_arr_.shape)
EPS = 0.000001 # sign
n = x1_arr_.shape[0]
for i in range(n):
x1v = x1_arr_[i, :].copy()
x2v = x2_arr_[i, :].copy()
v1_arr = iobj.implicitFunction(x1v[np.newaxis, :])[0]
v2_arr = iobj.implicitFunction(x2v[np.newaxis, :])[0]
active_indices = np.arange(0, n) # mid
iteration = 1
while True:
assert mysign_np(v2_arr) * mysign_np(
v1_arr) < 0 - EPS # greater or equal
assert v1_arr < 0 - ROOT_TOLERANCE
x_midpoint = (x1v[:] + x2v[:]) / 2.0
x_midpoint[3] = 1
v_mid_val = iobj.implicitFunction(x_midpoint[np.newaxis, :])[0]
on_boundary_boolean = np.abs(v_mid_val) <= ROOT_TOLERANCE #eq
boolean_outside = v_mid_val < -ROOT_TOLERANCE # gt
boolean_inside = v_mid_val > +ROOT_TOLERANCE # -v_mid_val < ROOT_TOLERANCE
assert boolean_outside or boolean_inside == (
not (on_boundary_boolean))
if on_boundary_boolean:
result_x_arr[i, :] = x_midpoint[:]
break
assert boolean_outside or boolean_inside
if boolean_inside:
v2_arr = v_mid_val
x2v[:] = x_midpoint[:]
if boolean_outside:
v1_arr = v_mid_val
x1v = x_midpoint
iteration += 1
assert x1v.shape == x2v.shape
assert np.all(np.abs(iobj.implicitFunction(result_x_arr)) < ROOT_TOLERANCE)
return result_x_arr
def hessianMatrix(self, p):
#warning: not tested
check_vector4_vectorized(p)
tp = np.dot(self.invmatrix, np.transpose(p))
tp = np.transpose(tp)
h1 = self.base_object.hessianMatrix(tp)
h = np.dot(h1, self.invmatrix) # which one is correct?
h = np.dot(self.invmatrix, np.tanspose(h1)) # which one is correct?
raise VirtualException()
return h
def evaluate_centroid_gradients(self, iobj):
assert self.centroids is not None
n = self.centroids.shape[0]
# centroids4 = np.concatenate( (self.centroids, np.ones((n,1))), axis=1)
centroids4 = self.centroids
check_vector4_vectorized(centroids4)
self.centroid_gradients = iobj.implicitGradient(centroids4)
assert not np.any(np.isnan(self.centroid_gradients))
assert not np.any(np.isinf(self.centroid_gradients))
self.centroid_normals = normalize_vector4_vectorized(
self.centroid_gradients)
def implicitFunction(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
#v = va * self.afactor + vb * self.bfactor + 1
fa = self.afactor
fb = self.bfactor
v = -(1 - (fa * np.exp(va) + fb * np.exp(vb)) / (fa + fb))
check_scalar_vectorized(v)
return v
def hessianMatrix(self, p):
#warning: not tested
check_vector4_vectorized(p)
#tp = np.dot(self.invmatrix, vec3.make_v4(np.transpose(p)))
#tp = np.transpose(tp)
h1 = self.base_object.hessianMatrix(tp)
#h = np.dot(h1, self.invmatrix) # which one is correct?
#h = np.dot(self.invmatrix, vec3.make_v4(np.tanspose(h1))) # which one is correct?
raise NotImplementedException()
return h
def hessianMatrix(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p) # why not used???
vb = self.b.implicitFunction(p)
c = np.greater(va, vb) # shape: (N,)
c = np.tile(np.expand_dims(c, axis=1), (1, 4))
ha = self.a.hessianMatrix(p)
hb = self.b.hessianMatrix(p)
h = ha * c + hb * (1-c)
check_matrix3_vectorized(h)
return h
def hessianMatrix(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = - self.b.implicitFunction(p)
c = 1 - np.greater(va, vb)
c = np.tile(c[:, np.newaxis, np.newaxis], (1, 4, 4))
assert c.shape[1:] == (4, 4)
ha = self.a.hessianMatrix(p)
hb = -self.b.hessianMatrix(p)
h = ha * c + hb * (1-c)
check_matrix3_vectorized(h)
return h
def implicitFunction(self, xa, return_grad=False):
check_vector4_vectorized(xa)
x4 = self._move(xa)
#return x4
v = self.base_object.implicitFunction(x4)
check_scalar_vectorized(v)
return v
if not return_grad:
return v
else:
raise NotImplementedError()
check_vector4_vectorized(g4)
return v, g4
def project_point_bidir_ohtake(iobj, start_x, lambda_val, max_dist):
""" max_dist is used.
See # setCenterOnSurface """
#""" lambda_val: step size"""
#max_iter = 20 # config["max_iter"]
check_vector4_vectorized(start_x)
assert start_x.shape[0] == 1
max_iter = 20
if FAST_VERSION:
p1 = search_near__ohtake_max_dist(iobj, start_x, None, lambda_val,
max_iter, max_dist)
else:
p1 = search_near__ohtake(iobj, start_x, None, lambda_val, max_iter)
if p1 is None:
return None # Should we return nothing if nothing found??
f1 = iobj.implicitFunction(p1)
# Mirror image: search the opposite way and accept only if it is closer than the best found.
p2 = 2 * start_x - p1
f2 = iobj.implicitFunction(p2)
p = p1 #None #p1 # None #p1 #default
if f1 * f2 < 0:
direction = (start_x - p1) # as in Ohtake
dn = np.linalg.norm(direction)
if dn > 0: #dn>0.000000001:
direction = direction / dn
#broken
if FAST_VERSION:
p3 = search_near__ohtake_max_dist(iobj, start_x, direction,
lambda_val, max_iter,
max_dist)
else:
p3 = search_near__ohtake(iobj, start_x, direction, lambda_val,
max_iter)
#no max_dist
if p3 is not None:
if np.linalg.norm(start_x - p3) > np.linalg.norm(start_x - p1):
p = p3
#else:
# p = p1
#else:
# p = p1
if p is not None:
if np.linalg.norm(start_x - p) > max_dist:
return None
return p
def implicitGradient(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
c = np.greater(va, vb) # shape: (N,)
c = np.tile(np.expand_dims(c, axis=1), (1, 4))
assert c.shape[1:] == (4,)
grada = self.a.implicitGradient(p)
gradb = self.b.implicitGradient(p)
grad = grada * c + gradb * (1-c)
check_vector4_vectorized(grad)
return grad
def func_test_bisection_p_vectorized(iobj, sp1, sp2, prop, max_iter_count):
global count_converged
global count_not_converged
#raise ImplementationError()
from basic_types import check_vector4
check_vector4_vectorized(sp1)
check_vector4_vectorized(sp2)
n = sp1.shape[0]
p1 = sp1.reshape((n, 4))
p2 = sp2.reshape((n, 4))
f1 = iobj.implicitFunction(p1)
f2 = iobj.implicitFunction(p2)
print "f1,f2: ", f1, f2
#p = bisection_prop_2(iobj, p1, p2, f1, f2, 20)
if prop:
converged, p, p2, jj = bisection_prop_2(iobj, p1, p2, f1, f2,
max_iter_count)
assert not p is None
else:
p, jj = bisection_3_standard_vectorized(iobj, p1, p2, f1, f2,
max_iter_count)
converged = p is not None
if not converged:
count_not_converged += 1
if prop:
if VERBOSE:
print "*** bisection_prop_2 did not converge. Hence the result is an interval" # this happens
print(p, p2, jj)
if not prop:
if VERBOSE:
print "*** bisection_standard did not converge. Hence the result is None"
print p, jj
else:
print(count_converged)
count_converged += 1
if VERBOSE:
print "converged: p", p
print "iteration: ", jj
from basic_types import make_vector4_vectorized
p4 = make_vector4_vectorized(p[0, 0], p[0, 1], p[0, 2])
f = iobj.implicitFunction(p4)
if VERBOSE:
print("f(p)=", f)
print("Total distance travelled: ", np.linalg.norm(sp1 - p),
np.linalg.norm(sp2 - p))
def implicitGradient(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
c = 1 - np.greater(va, vb)
c = np.tile(c[:, np.newaxis], (1, 4))
assert c.shape[1:] == (4,)
grada = self.a.implicitGradient(p)
gradb = self.b.implicitGradient(p)
grad = grada * c + gradb * (1-c)
check_vector4_vectorized(grad)
return grad
def implicitGradient(self, p):
check_vector4_vectorized(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
ca = np.tile(va[:, np.newaxis], (1, 4))
cb = np.tile(vb[:, np.newaxis], (1, 4))
grada = self.a.implicitGradient(p)
gradb = self.b.implicitGradient(p)
#not tested
fa = self.afactor
fb = self.bfactor
grad = +(fa * np.exp(ca) * grada + fb * np.exp(cb) * gradb) / (fa + fb)
grad[:, 3] = 1
check_vector4_vectorized(grad)
return grad
def numerical_gradient_vectorized_v1(iobj, x):
check_vector4_vectorized(x)
if TEST_ON:
print "numerical_gradient1",
flush()
count = x.shape[0]
g = np.zeros((count, 4))
for i in range(x.shape[0]):
v = x[i, 0:4]
# inefficient: not vectorised
g[i, :] = numerical_gradient(iobj, v, is_vectorized=True)
assert not np.any(np.isnan(g), axis=None)
print "numerical_gradient2",
flush()
g2 = numerical_gradient_vectorized_v2(iobj, x.copy())
if TEST_ON:
assert np.allclose(g, g2)
print "done"
flush()
return g2
def implicitFunction(self, p):
check_vector4_vectorized(p)
sides = len(self.p0)
n = p.shape[0]
temp = np.zeros((n, sides))
for i in range(sides):
p0 = self.p0[i]
n0 = self.n0[i]
n0[3] = 0
sub = p - np.tile(p0[np.newaxis, :], (n, 1))
# assert np.allclose(sub[:, 3], 0)
vi = np.dot(sub, n0)
temp[:, i] = vi
#ia = np.argmin(temp, axis=1)
va = np.amin(temp, axis=1)
return va
"""
n = p.shape[0]
#temp = np.zeros((n,6,3))
mp0 = np.zeros((1,6,3))
for i in range(len(self.p0)):
p0 = self.p0[i]
#n0 = self.n0[i]
mp0[0,i,:] = - p0
#temp = np.zeros((n,6,3))
temp = np.tile(p[:,np.newaxis,:], (1,6,1))
temp = temp - np.tile(mp0, (n,:,:)) # x-p0
temp = np.dot( temp, n0)
for i in range(len(self.p0)):
p0 = self.p0[i]
n0 = self.n0[i]
temp[:,i,:] = p[i,0:3] - p0
#vi = np.dot(x-p0, n0)
pass
"""
"""
def _move(self, xa):
x = xa[:, 0:3].copy()
pointcount = x.shape[0]
assert self.dir_normalised.ndim == 1
assert x.shape[1] == self.dir_normalised.size
x = x - self.start[np.newaxis, :]
la = np.dot(x, self.dir_normalised)
assert la.shape == (pointcount, )
la = np.floor(la / self.dir_len)
print la.shape
la[la > self.tilecount - 1 - self.back_count + 0.1] = 0
la[la < -self.back_count - 0.1] = 0
la = la * self.dir_len
print self.dir_normalised.shape
x = x - np.dot(la[:, np.newaxis],
self.dir_normalised[np.newaxis, :]) # inplace
x = x + self.start[np.newaxis, :]
assert x.shape == (pointcount, 3)
x4 = np.concatenate((x, np.ones((pointcount, 1))), axis=1)
check_vector4_vectorized(x4)
return x4
def implicitGradient(self, p): # -> Vector3D :
check_vector4_vectorized(p)
tp = np.dot(self.invmatrix, np.transpose(p))
tp = np.transpose(tp)
g = self.base_object.implicitGradient(tp)
check_vector4_vectorized(g)
g[:, 3] = 0 # important
v4 = np.dot(np.transpose(self.invmatrix), np.transpose(g))
v4 = np.transpose(v4) # not efficient
v4[:, 3] = 1
check_vector4_vectorized(v4)
return v4
def implicitGradient(self, pa): # -> Vector3D :
check_vector4_vectorized(pa)
tp = np.dot(self.invmatrix, np.transpose(pa))
tp = np.transpose(tp) # inefficient
g = self.sphere.implicitGradient(tp)
check_vector4_vectorized(g) # not needed
#print("g: ", g)
g[:, 3] = 0 # important
v4 = np.dot(np.transpose(self.invmatrix), np.transpose(g))
v4 = np.transpose(v4) # not efficient
#print("v4: ", v4)
v4[:, 3] = 1
check_vector4_vectorized(v4)
return v4
def implicitGradient__(self, p): # -> Vector3D :
check_vector4_vectorized(p)
p = np.concatenate((p, np.ones((p.shape[0], 1))), axis=1)
tp = np.dot(self.invmatrix, np.transpose(p))
tp = np.transpose(tp)
#tp = tp[:, :3]
g = self.base_object.implicitGradient(tp)
check_vector4_vectorized(g)
g = np.concatenate((g, np.ones((g.shape[0], 1))), axis=1)
v4 = np.dot(np.transpose(self.invmatrix), (np.transpose(g)))
v4 = np.transpose(v4)
#v3 = v4[:, :3]
check_vector4_vectorized(v4)
return v4
