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