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
