def interpolate(R1, R2, u):
"""Interpolate linearly between the two rotations R1 and R2. """
R = mul(inv(R1), R2)
m = moment(R)
angle = vectorops.norm(m)
if angle == 0: return R1
axis = vectorops.div(m, angle)
return mul(R1, rotation(axis, angle * u))
def interpolate(R1,R2,u):
"""Interpolate linearly between the two rotations R1 and R2. """
R = mul(inv(R1),R2)
m = moment(R)
angle = vectorops.norm(m)
if angle==0: return R1
axis = vectorops.div(m,angle)
return mul(R1,rotation(axis,angle*u))
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 triangle(a,b,c,lighting=True):
if lighting:
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
glBegin(GL_TRIANGLES)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd();
def triangle(a,b,c,lighting=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically"""
if lighting:
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
glBegin(GL_TRIANGLES)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd();
def triangle(a, b, c, lighting=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically"""
if lighting:
n = vectorops.cross(vectorops.sub(b, a), vectorops.sub(c, a))
n = vectorops.mul(n, 1.0 / vectorops.norm(n))
glNormal3f(*n)
glBegin(GL_TRIANGLES)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd()
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
scale = 2.0*math.tan(rfov*0.5/aspect)*aspect
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
scale = 2.0*math.tan(rfov*0.5/aspect)*aspect
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
scale = math.tan(self.fov*math.pi/180.0)/self.height
#HACK: I don't know why this seems to work!
scale *= 0.925
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def triangle(a,b,c,lighting=True,filled=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically. If filled=true, it is drawn filled,
otherwise, it is drawn as a wireframe."""
if lighting:
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
if filled:
glBegin(GL_TRIANGLES)
else:
glBegin(GL_LINE_LOOP)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd();
def triangle(a, b, c, lighting=True, filled=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically. If filled=true, it is drawn filled,
otherwise, it is drawn as a wireframe."""
if lighting:
n = vectorops.cross(vectorops.sub(b, a), vectorops.sub(c, a))
n = vectorops.mul(n, 1.0 / vectorops.norm(n))
glNormal3f(*n)
if filled:
glBegin(GL_TRIANGLES)
else:
glBegin(GL_LINE_LOOP)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd()
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glLineWidth(2.0)
gldraw.xform_widget(tlocal,self.attributes.get("length",0.1),self.attributes.get("width",0.01))
glLineWidth(1.0)
#draw curve between frame and parent
if item.parent() != None:
d = vectorops.norm(tlocal[1])
vlen = d*0.5
v1 = so3.apply(tlocal[0],[-vlen]*3)
v2 = [vlen]*3
#glEnable(GL_BLEND)
#glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
#glColor4f(1,1,0,0.5)
glColor3f(1,1,0)
gldraw.hermite_curve(tlocal[1],v1,[0,0,0],v2,0.03)
#glDisable(GL_BLEND)
glEnable(GL_DEPTH_TEST)
def momentToMatrix(m):
"""Converts an exponential map rotation represenation m to a matrix R"""
angle = vectorops.norm(m)
axis = vectorops.div(m,angle)
return so3.rotation(axis,angle)
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
def from_moment(w):
"""Converts a moment representation w to a 3D rotation matrix."""
length = vectorops.norm(w)
if length < 1e-7: return identity()
return rotation(vectorops.mul(w,1.0/length),length)
def axis_angle(R):
"""Returns the (axis,angle) pair representing R"""
m = moment(R)
return (vectorops.unit(m),vectorops.norm(m))
def distance(self,a,b):
return vectorops.norm(se3.error((a[:9],a[9:]),(b[:9],b[9:])))
def distance(self,a,b):
return vectorops.norm(so3.error(a,b))
def distance(self, a, b):
return vectorops.norm(so3.error(a, b))
def from_moment(w):
"""Converts a moment representation w to a 3D rotation matrix."""
length = vectorops.norm(w)
if length < 1e-7: return identity()
return rotation(vectorops.mul(w, 1.0 / length), length)
def axis_angle(R):
"""Returns the (axis,angle) pair representing R"""
m = moment(R)
return (vectorops.unit(m), vectorops.norm(m))
def distance(self, a, b):
return vectorops.norm(se3.error((a[:9], a[9:]), (b[:9], b[9:])))