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 segment_closest_point(s, x): """Given a segment s=(a,b) and a point x, returns a tuple containing the closest point parameter in [0,1] and the closest point on s""" dir = vectorops.sub(s[1], s[0]) d = vectorops.sub(x, s[0]) dp = vectorops.dot(d, dir) dnorm2 = vectorops.dot(dir, dir) if dp < 0: return (0.0, s[0]) if dp > dnorm2: return (1.0, s[1]) proj = vectorops.add(s[0], vectorops.mul(dir, dp / dnorm2)) return (dp / dnorm2, proj)
def mul(R1,R2): """Multiplies two rotations.""" m1=matrix(R1) m2T=matrix(inv(R2)) mres = matrix(identity()) for i in xrange(3): for j in xrange(3): mres[i][j] = vectorops.dot(m1[i],m2T[j]) #print "Argument 1" #print __str__(R1) #print "Argument 2" #print __str__(R2) #print "Result" R = from_matrix(mres) #print __str__(R) return R
def mul(R1, R2): """Multiplies two rotations.""" m1 = matrix(R1) m2T = matrix(inv(R2)) mres = matrix(identity()) for i in xrange(3): for j in xrange(3): mres[i][j] = vectorops.dot(m1[i], m2T[j]) #print "Argument 1" #print __str__(R1) #print "Argument 2" #print __str__(R2) #print "Result" R = from_matrix(mres) #print __str__(R) return R
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 click_robot(self,x,y): """Helper: returns a list of robot objects sorted in order of increasing distance.""" #get the viewport ray (s,d) = self.click_ray(x,y) #run the collision tests collided = [] for l in range(self.robot.numLinks()): (hit,pt) = self.robot.link(l).geometry().rayCast(s,d) if hit: dist = vectorops.dot(vectorops.sub(pt,s),d) collided.append((dist,l)) if len(collided) == 0: return id = collided[0][1] if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def click_world(self,x,y): """Helper: returns a list of world objects sorted in order of increasing distance.""" #get the viewport ray (s,d) = self.click_ray(x,y) #run the collision tests collided = [] for g in self.collider.geomList: (hit,pt) = g[1].rayCast(s,d) if hit: dist = vectorops.dot(vectorops.sub(pt,s),d) collided.append((dist,g[0])) if len(collided) == 0: return id = collided[0][1].getID() if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def click_robot(self, x, y): """Helper: returns a list of robot objects sorted in order of increasing distance.""" #get the viewport ray (s, d) = self.click_ray(x, y) #run the collision tests collided = [] for l in range(self.robot.numLinks()): (hit, pt) = self.robot.link(l).geometry().rayCast(s, d) if hit: dist = vectorops.dot(vectorops.sub(pt, s), d) collided.append((dist, l)) if len(collided) == 0: return id = collided[0][1] if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def click_world(self, x, y): """Helper: returns a list of world objects sorted in order of increasing distance.""" #get the viewport ray (s, d) = self.click_ray(x, y) #run the collision tests collided = [] for g in self.collider.geomList: (hit, pt) = g[1].rayCast(s, d) if hit: dist = vectorops.dot(vectorops.sub(pt, s), d) collided.append((dist, g[0])) if len(collided) == 0: return id = collided[0][1].getID() if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def readIKObjective(text): """Reads an IKObjective from a string in the Klamp't native format 'link destLink posConstraintType [pos constraint items] ... rotConstraintType [rot constraint items]' where link and destLink are integers, posConstraintType is one of - N: no constraint - P: position constrained to a plane - L: position constrained to a line - F: position constrained to a point and rotConstraintType is one of - N: no constraint - T: two-axis constraint (not supported) - A: rotation constrained about axis - F: fixed rotation The [pos constraint items] contain a variable number of whitespace- separated items, dependending on posConstraintType: - N: 0 items - P: the local position xl yl zl, world position x y z on the plane, and plane normal nx,ny,nz - L: the local position xl yl zl, world position x y z on the line, and line axis direction nx,ny,nz - F: the local position xl yl zl and world position x y z The [rot constraint items] contain a variable number of whitespace- separated items, dependending on rotConstraintType: - N: 0 items - T: not supported - A: the local axis xl yl zl and the world axis x y z - F: the world rotation matrix, in moment (aka exponential map) form mx my mz (see so3.from_moment() """ items = text.split() if len(items) < 4: raise ValueError("Not enough items to unpack") link = int(items[0]) destlink = int(items[1]) ind = 2 posType = None posLocal = None posWorld = None posDirection = None rotType = None rotWorld = None rotAxis = None #read pos constraint posType = items[ind] ind += 1 if posType=='N': #no constraint pass elif posType=='P' or posType=='L': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 posDirection = items[ind:ind+3] ind += 3 elif posType=='F': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid pos type "+posType+", must be N,P,L or F") rotType = items[ind] ind += 1 if rotType=='N': #no constraint pass elif rotType=='T' or rotType=='A': rotAxis = items[ind:ind+3] ind += 3 rotWorld = items[ind:ind+3] ind += 3 elif rotType=='F': rotWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid rot type "+rotType+", must be N,T,A or F") if posLocal: posLocal = [float(v) for v in posLocal] if posWorld: posWorld = [float(v) for v in posWorld] if posDirection: posDirection = [float(v) for v in posDirection] if rotLocal: rotLocal = [float(v) for v in rotLocal] if rotWorld: rotWorld = [float(v) for v in rotWorld] obj = IKObjective() obj.setLinks(link,destLink); if posType=='N': obj.setFreePosConstraint() elif posType=='F': obj.setFixedPosConstraint(posLocal,posWorld) elif posType=='P': obj.setPlanePosConstraint(posLocal,posDirection,vectorops.dot(posDirection,posWorld)) else: obj.setLinearPosConstraint(posLocal,posWorld,posDirection) if rotType == 'N': obj.setFreeRotConstraint() elif rotType == 'F': #fixed constraint R = so3.from_moment(rotWorld) obj.setFixedRotConstraint(R) elif rotType == 'A': obj.setAxialRotConstraint(rotLocal,rotWorld) else: raise NotImplementedError("Two-axis rotational constraints not supported") return obj