def rotation_matrix(angle, along, units="rad", around=None):
from lib import transformations as tf
if units == "rad":
pass
elif units == "deg":
angle = angle / 360.0 * np.pi * 2.0
else:
raise UsageError()
if around is None:
# center = 0
# t = np.eye(4)
pass
else:
check_vector4(around)
center = around
# print "center=",center
t = np.eye(4)
t[:, 3] = center
assert t[3, 3] == 1.
tinv = np.eye(4)
tinv[:, 3] = -center
tinv[3, 3] = 1
# print "t", t
# print "tinv", tinv
check_vector4(along)
rm = tf.rotation_matrix(angle, along[0:3])
if around is None:
return rm
else:
return np.dot(np.dot(rm, tinv), t)
def implicitGradient(self, p): # -> Vector3D :
check_vector4(p)
tp = np.dot(self.invmatrix, p)
g = self.base_object.implicitGradient(tp)
g[3] = 0 # important
v4 = np.dot(np.transpose(self.invmatrix), g)
v4[3] = 1
return v4
def hessianMatrix(self, p):
check_vector4(p)
ha = self.a.hessianMatrix(p)
hb = self.b.hessianMatrix(p)
h = ha * self.afactor + hb * self.bfactor
#todo
check_matrix3(h)
return h
def hessianMatrix(self, p):
check_vector4(p)
h = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]], ndmin=2)
check_matrix3(h)
#todo : array of matrix3s
n = p.shape[0]
ha = np.tile(h[np.newaxis, :, :], (n, 1, 1))
return h
def hessianMatrix(self, p):
#warning: not tested
check_vector4(p)
tp = np.dot(self.invmatrix, p)
h1 = self.base_object.hessianMatrix(tp)
h = np.dot(h1, self.invmatrix) # which one is correct?
h = np.dot(self.invmatrix, h1) # which one is correct?
raise VirtualException()
return h
def implicitFunction(self, p):
check_vector4(p)
va = self.a.implicitFunction(p)
vb = - self.b.implicitFunction(p)
if va > vb:
v = vb
else:
v = va
return v
def hessianMatrix(self, p):
check_vector4(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
if va > vb:
h = self.b.hessianMatrix(p)
else:
h = self.a.hessianMatrix(p)
check_matrix3(h)
return h
def implicitGradient(self, p):
check_vector4(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
if va > vb:
grad = self.b.implicitGradient(p)
else:
grad = self.a.implicitGradient(p)
check_vector4(grad)
return grad
def implicitFunction(self, p):
check_vector4(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))
return v
def ellipsoid_point_and_gradient(self, m, x, correctGrad, center=None):
""" Checks if the point x is on the surface, and if the gradient is correct."""
e = nonvec.Ellipsoid(m)
v = e.implicitFunction(x)
g = e.implicitGradient(x)
check_vector4(g)
correctScalar = 0
assert np.abs(v - correctScalar) < TOLERANCE, ("Implicit Function's scalar value incorrect: %2.20f" % (v,))
#assert np.allclose( g, correctGrad , atol=TOLERANCE ) , "Incorrect gradient"
msg = "nonvec.Ellipsoid(m): "+str(e)
(are_parallel,are_directed) = vectors_parallel_and_direction(g[0:3], correctGrad[0:3])
self.assertTrue(are_parallel, "Incorrect gradient: not parallel "+msg)
self.assertTrue( are_directed, "parallel but opposite directions "+msg)
def implicitFunction(self, p):
check_vector4(p)
chosen_i = None
v = +np.infty
for i in range(len(self.p0)):
p0 = self.p0[i]
n0 = self.n0[i]
vi = np.dot(p - p0, n0)
if vi < v:
v = vi
chosen_i = i
assert not chosen_i is None
return v
def implicitGradient(self, p):
check_vector4(p)
va = self.a.implicitFunction(p)
vb = self.b.implicitFunction(p)
ca = va #np.tile( va[:,np.newaxis], (1,4) )
cb = vb #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(grad)
return grad
def side(x, y, z):
p0 = (make_vector4(x, y, z) + 0.0)
p0 = p0 / 2.0 * size
n0 = -make_vector4(x, y, z)
n0 = n0
self.p0 += [p0]
self.n0 += [n0]
self.p0[-1][3] = 1
self.n0[-1][3] = 1
#print(self.p0[-1])
check_vector4(self.p0[-1])
check_vector4(self.n0[-1])
def norm2(v):
return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]
assert norm2(self.n0[-1][0:3]) - 1 == 0.0
def func_test_bisection_p(iobj, sp1, sp2, prop, max_iter_count):
global count_converged
global count_not_converged
from basic_types import check_vector4
check_vector4(sp1)
check_vector4(sp2)
p1 = sp1.reshape((1, 4))
p2 = sp2.reshape((1, 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(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 rotate(self, angle, along, units="rad"):
if units == "rad":
pass
elif units == "deg":
angle = angle / 360.0 * np.pi * 2.0
else:
raise UsageError() # UsageError
check_vector4(along)
rm = tf.rotation_matrix(angle, along[0:3])
self.matrix = np.dot(rm, self.matrix)
self.invmatrix = make_inverse(self.matrix)
#print(angle /(3.1415926536*2) * 360 )
#print(rm)
#print(self.matrix)
return self
def implicitGradient(self, p):
check_vector4(p)
chosen_i = None
v = +np.infty
for i in range(len(self.p0)):
p0 = self.p0[i]
n0 = self.n0[i]
vi = np.dot(p - p0, n0)
if vi < v:
v = vi
chosen_i = i
grad = n0
assert not chosen_i is None
#not tested
grad[3] = 1
check_vector4(grad)
return grad
def test_ellipsoid_random_points(self):
"""Testing hundreds of random points on a sphere of size RADIUS=3"""
for i in range(0, 100):
RADIUS = 3
POW = 4 # higher POW will get points more toward parallel to axes
rcenter = make_random_vector(1000, 1.0)[0:3]
r0 = make_random_vector(RADIUS, POW)[0:3]
r = r0 + rcenter
assert r.shape[0] == 3
x = 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 = make_vector4(-r0[0], -r0[1], -r0[2])
check_vector4(expected_grad)
self.ellipsoid_point_and_gradient(m, x, expected_grad)
def optimize_vertex(vert, iobj, radius_of_max_change):
v = make_vector4(vert[0], vert[1], vert[2]) # inefficient
check_vector4(v)
#f = iobj.implicitFunction(v)
#g = iobj.implicitGradient(v)
#v += g * np.random.rand() * radius_of_max_change * 2
#v[3] = 1
iterations = 2 #14
for i in range(iterations):
f = iobj.implicitFunction(v)
g = iobj.implicitGradient(v)
tau = 0.1 # * 3
a = 1
z_force = -tau * a * f * g
v += z_force
v[3] = 1
return v[0:3]
def __init__(self, A, w, u, radius_u, radius_v, c_len):
""" The cross section of a SimpleCylinder is always a circle.
"""
(self.A, self.w, self.u, self.radius_u, self.radius_v, self.c_len) = \
(A, w, u, radius_u, radius_v, c_len)
check_vector4(w)
check_vector4(u)
check_vector4(A)
assert w[3] == 1
assert u[3] == 1
assert A[3] == 1
w = w[:3]
u = u[:3]
A = A[:3]
v = np.cross(u[:3], w[:3])
assert w.shape == (3,)
assert u.shape == (3,)
assert v.shape == (3,)
assert A.shape == (3,)
self.u = u[:, np.newaxis]
self.v = v[:, np.newaxis]
self.w = w[:, np.newaxis]
self.A = A[:, np.newaxis]
assert self.u.shape == (3, 1)
assert self.v.shape == (3, 1)
assert self.w.shape == (3, 1)
assert self.A.shape == (3, 1)
assert np.isscalar(radius_u)
assert np.isscalar(radius_v)
norm_tol = 0.00000001 # 1000 km
assert np.abs(np.linalg.norm(w)-1.0) < norm_tol
assert np.abs(np.linalg.norm(u)-1.0) < norm_tol
self.UVW = np.concatenate( (self.u, self.v, self.w), axis=1 )
self.UVW_inv = np.linalg.inv( self.UVW )
assert self.c_len > 0
assert self.integrity_invariant()
def hessianMatrix(self, p):
check_vector4(p)
h = np.array([[-2, 0, 0], [0, -2, 0], [0, 0, -2]], ndmin=2)
check_matrix3(h)
return h
def implicitGradient(self, p):
check_vector4(p)
grad = -2 * p
grad[3] = 1
check_vector4(grad)
return grad
def implicitFunction(self, p):
check_vector4(p)
return 1.0 - (np.dot(p[:3], p[:3]))
def implicitFunction(self, p):
check_vector4(p)
tp = np.dot(self.invmatrix, p)
v = self.base_object.implicitFunction(tp)
return v
def numerical_gradient(iobj,
pos0,
delta_t=0.01 / 10.0 / 10.0,
order=5,
is_vectorized="unspecified"):
#0.1 is not enough for delta_t
assert is_vectorized != "unspecified"
if is_vectorized:
check_vector4(pos0) # incorrect
#assert False
#check_vector4_vectorized(pos0)
assert issubclass(type(iobj), vectorized.ImplicitFunctionVectorized)
else:
check_vector4(pos0)
assert issubclass(type(iobj), nonvec.ImplicitFunctionPointwise)
m = order # sample points: -m,...,-1,0,1,2,...,+m
_VERBOSE = False
from lib import finite_diff_weights
sample_points = range(-m, m + 1)
n = m * 2 + 1
x0 = 0
findiff_weights = finite_diff_weights.weights(k=1,
x0=x0,
xs=np.array(sample_points) *
delta_t)
del x0
pos0_4 = repeat_vect4(1, pos0)
pos = np.tile(pos0_4, (3 * n, 1))
assert not issubclass(pos.dtype.type, np.integer)
if pos.shape[0] in [1, 1718772L]:
set_trace()
dx = repeat_vect4(1, make_vector4(1, 0, 0))
dy = repeat_vect4(1, make_vector4(0, 1, 0))
dz = repeat_vect4(1, make_vector4(0, 0, 1))
dxyz = [dx, dy, dz]
ci = 0
for d in range(3):
for i in sample_points:
dd = dxyz[d]
if pos.shape[0] == 1 and ci == 1:
set_trace()
pos[ci, :] = pos[ci, :] + (dd * delta_t * float(i))
#w[ci] = findef(i,n)
ci += 1
pos[:, 3] = 1
if is_vectorized:
v = iobj.implicitFunction(pos) # v .shape: (3,11)
else:
v = np.zeros((pos.shape[0], ))
for i in range(pos.shape[0]):
v1 = iobj.implicitFunction(pos[i, :]) # v .shape: (3,11)
v[i] = v1
v3 = np.reshape(v, (3, n), order='C') # v3 .shape: (11,)
#print( np.diff(v, axis=0) / delta_t )
#print( np.diff(v3, axis=1) / delta_t )
#Lipchitz_L
#Lipschitz constant = Lipchitz_B
Lipchitz_B = 50
Lipchitz_beta = 1 # order. Keep it 1
#H\:older continuous: |f(h)-f(0)| <= B|h|^beta
b_h_beta = Lipchitz_B * (np.abs(delta_t)**Lipchitz_beta)
#print("v3=",v3)
#print("diff=",np.diff(v3, axis=1) )
#print(b_h_beta)
d0 = np.abs(np.diff(v3, axis=1))
#lipschitz_condition = d <= b_h_beta
nonsmooth_ness = d0 / (np.abs(delta_t)**Lipchitz_beta)
#nonsmooth_ness2 = np.mean(nonsmooth_ness, axis=1)
d = np.abs(np.diff(v3, n=1, axis=1)) / np.abs(delta_t)
d = d - np.tile(np.mean(d, axis=1, keepdims=True), (1, d.shape[1]))
d = np.abs(d) / np.abs(delta_t)
d = d - np.tile(np.mean(d, axis=1, keepdims=True), (1, d.shape[1]))
#d = np.abs(d)
#d = np.abs( np.diff(d, n=1, axis=1) ) / np.abs(delta_t)
#nonsmooth_ness = d / (np.abs(delta_t)**Lipchitz_beta)
#if(np.max(np.ravel(d))) > 50:
# print("warning")
# print(nonsmooth_ness)
#print(d)
if (np.max(np.ravel(nonsmooth_ness))) > 100 * 10:
print "warning: nonsmooth ",
#print(nonsmooth_ness) # lots of zeros and one big value
""" Calculating the numerical derivative using finite difference (convolution with weights) """
#convolusion
grad_cnv = np.dot(v3, findiff_weights)
#grad_cnv = np.reshape(grad_cnv, (1,3))[:,np.newaxis]
#grad_cnv = np.concatenate( ( grad_cnv[np.newaxis,:], np.reshape(np.array([1]),(1,1)) ), axis=1)
def v3_to_v14(v):
""" Converts shape from (3,) into a (1,4) vector4 """
assert v.ndim == 1
return np.concatenate(
(v[np.newaxis, :], np.reshape(np.array([1]), (1, 1))), axis=1)
grad_cnv = v3_to_v14(grad_cnv)
#print("weights: ",findiff_weights)
#Detecting sharp edges (non-smooth points, i.e. corners and edges and ridges)
if np.max(np.abs(grad_cnv)) > 100:
pass
#print("******* max(grad) > 100")
#print(np.abs(grad_cnv))
#else:
# print(np.abs(grad_cnv))
if _VERBOSE:
#np.set_printoptions( precision=9 )
np.set_printoptions(
formatter={'all': lambda x: '' + ("%2.19f" % (x, ))})
""" Calculating the numerical derivative using 'mean of diff' """
grad_mean = np.mean(-np.diff(v3, axis=1) / delta_t, axis=1)
if _VERBOSE:
print("grad_mean: ", grad_mean)
print("grad_convolusion: ", grad_cnv)
if False:
g = iobj.implicitGradient(pos0_4)
if _VERBOSE:
print("grad_analytical: ", g)
#print( grad_cnv.shape )
#print( g.shape )
#print( grad_mean.shape )
print("Errors:")
print("conv error: ", g - grad_cnv)
#Amazing precision: [[0.0000000000001995071 -0.0000000038590481921 0.0000000000000008882 0.0000000000000000000]]
print("mean error: ", g - v3_to_v14(grad_mean))
#Terrible error: [-0.262 2.12266 0 ]
#v3 * findiff_weights
print("to be continued")
assert not np.any(np.isnan(grad_cnv.ravel()))
return grad_cnv
