def make_outline(self):
left = []
right = []
for i in xrange(len(self.path)):
s1l = None
s1r = None
s2l = None
s2r = None
if i > 0:
d = vectorops.unit(
vectorops.sub(self.path[i], self.path[i - 1]))
ofs = (self.gutter * d[1], -self.gutter * d[0])
s1l = (d, ofs)
s1r = (d, (-ofs[0], -ofs[1]))
if i + 1 < len(self.path):
d = vectorops.unit(
vectorops.sub(self.path[i + 1], self.path[i]))
ofs = (self.gutter * d[1], -self.gutter * d[0])
s2l = (d, ofs)
s2r = (d, (-ofs[0], -ofs[1]))
if i == 0:
left.append(tuple(vectorops.add(self.path[i], s2l[1])))
right.append(tuple(vectorops.add(self.path[i], s2r[1])))
elif i + 1 == len(self.path):
left.append(tuple(vectorops.add(self.path[i], s1l[1])))
right.append(tuple(vectorops.add(self.path[i], s1r[1])))
else:
#figure out intersections
if vectorops.dot(s1r[0], s2r[1]) < 0:
#inner junction, find line intersection
right.append(
tuple(
vectorops.add(
self.path[i],
ray_ray_intersection(s1r[1], s1r[0], s2r[1],
s2r[0]))))
else:
#outer junction
right.append(tuple(vectorops.add(self.path[i], s1r[1])))
right.append(tuple(vectorops.add(self.path[i], s2r[1])))
if vectorops.dot(s1l[0], s2l[1]) < 0:
#inner junction, find line intersection
left.append(
tuple(
vectorops.add(
self.path[i],
ray_ray_intersection(s1l[1], s1l[0], s2l[1],
s2l[0]))))
else:
#outer junction
left.append(tuple(vectorops.add(self.path[i], s1l[1])))
left.append(tuple(vectorops.add(self.path[i], s2l[1])))
self.outline = left + list(reversed(right))
def sample():
"""Returns a uniformly distributed rotation matrix."""
import random
q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return from_moment(m)
def sample():
"""Returns a uniformly distributed rotation matrix."""
import random
q = [
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)
]
q = vectorops.unit(q)
theta = math.acos(q[3]) * 2.0
if abs(theta) < 1e-8:
m = [0, 0, 0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]), theta)
return from_moment(m)
def vector_rotation(v1,v2):
"""Finds the minimal-angle matrix that rotates v1 to v2. v1 and v2
are assumed to be nonzero"""
a1 = vectorops.unit(v1)
a2 = vectorops.unit(v2)
cp = vectorops.cross(a1,a2)
dp = vectorops.dot(a1,a2)
if abs(vectorops.norm(cp)) < 1e-4:
if dp < 0:
R0 = canonical(a1)
#return a rotation 180 degrees about the canonical y axis
return rotation(R0[3:6],math.pi)
else:
return identity()
else:
angle = math.acos(max(min(dp,1.0),-1.0))
axis = vectorops.mul(cp,1.0/vectorops.norm(cp))
return rotation(axis,angle)
def vector_rotation(v1, v2):
"""Finds the minimal-angle matrix that rotates v1 to v2. v1 and v2
are assumed to be nonzero"""
a1 = vectorops.unit(v1)
a2 = vectorops.unit(v2)
cp = vectorops.cross(a1, a2)
dp = vectorops.dot(a1, a2)
if abs(vectorops.norm(cp)) < 1e-4:
if dp < 0:
R0 = canonical(a1)
#return a rotation 180 degrees about the canonical y axis
return rotation(R0[3:6], math.pi)
else:
return identity()
else:
angle = math.acos(max(min(dp, 1.0), -1.0))
axis = vectorops.mul(cp, 1.0 / vectorops.norm(cp))
return rotation(axis, angle)
def quaternion(R):
"""Given a Klamp't rotation representation, produces the corresponding
unit quaternion (w,x,y,z)."""
tr = trace(R) + 1.0
a11, a21, a31, a12, a22, a32, a13, a23, a33 = R
#If the trace is nonzero, it's a nondegenerate rotation
if tr > 1e-5:
s = math.sqrt(tr)
w = s * 0.5
s = 0.5 / s
x = (a32 - a23) * s
y = (a13 - a31) * s
z = (a21 - a12) * s
return vectorops.unit((w, x, y, z))
else:
#degenerate it's a rotation of 180 degrees
nxt = [1, 2, 0]
#check for largest diagonal entry
i = 0
if a22 > a11: i = 1
if a33 > max(a11, a22): i = 2
j = nxt[i]
k = nxt[j]
M = matrix(R)
q = [0.0] * 4
s = math.sqrt((M[i][i] - (M[j][j] + M[k][k])) + 1.0)
q[i] = s * 0.5
if abs(s) < 1e-7:
raise ValueError(
"Could not solve for quaternion... Invalid rotation matrix?")
else:
s = 0.5 / s
q[3] = (M[k][j] - M[j][k]) * s
q[j] = (M[i][j] + M[j][i]) * s
q[k] = (M[i][k] + M[i][k]) * s
w, x, y, z = q[3], q[0], q[1], q[2]
return vectorops.unit([w, x, y, z])
def quaternion(R):
"""Given a Klamp't rotation representation, produces the corresponding
unit quaternion (w,x,y,z)."""
tr = trace(R) + 1.0;
a11,a21,a31,a12,a22,a32,a13,a23,a33 = R
#If the trace is nonzero, it's a nondegenerate rotation
if tr > 1e-5:
s = math.sqrt(tr)
w = s * 0.5
s = 0.5 / s
x = (a32 - a23) * s
y = (a13 - a31) * s
z = (a21 - a12) * s
return vectorops.unit((w,x,y,z))
else:
#degenerate it's a rotation of 180 degrees
nxt = [1, 2, 0]
#check for largest diagonal entry
i = 0
if a22 > a11: i = 1
if a33 > max(a11,a22): i = 2
j = nxt[i]
k = nxt[j]
M = matrix(R)
q = [0.0]*4
s = math.sqrt((M[i][i] - (M[j][j] + M[k][k])) + 1.0);
q[i] = s * 0.5
if abs(s)<1e-7:
raise ValueError("Could not solve for quaternion... Invalid rotation matrix?")
else:
s = 0.5 / s;
q[3] = (M[k][j] - M[j][k]) * s;
q[j] = (M[i][j] + M[j][i]) * s;
q[k] = (M[i][k] + M[i][k]) * s;
w,x,y,z = q[3],q[0],q[1],q[2]
return vectorops.unit([w,x,y,z])
def axis_angle(R):
"""Returns the (axis,angle) pair representing R"""
m = moment(R)
return (vectorops.unit(m),vectorops.norm(m))
def axis_angle(R):
"""Returns the (axis,angle) pair representing R"""
m = moment(R)
return (vectorops.unit(m), vectorops.norm(m))
def expmap_from_euler(ai, aj, ak, axes="sxyz"):
matrix = transformations.euler_matrix(ai, aj, ak, axes)
R = so3.from_matrix(matrix)
moment = so3.moment(R)
axis_angle_parameters = vectorops.unit(moment) + [vectorops.norm(moment)]
return axis_angle_parameters
