def deriv(self,t,endBehavior='loop'): """Evaluates the trajectory velocity using piecewise linear interpolation""" i,u = self.getSegment(t,endBehavior) if i<0: return [0.0]*len(self.milestones[0]) elif i>=len(self.milestones): return [0.0]*len(self.milestones[-1]) return vectorops.div(self.difference(self.milestones[i+1],self.milestones[i]),1.0/(self.times[i+1]-self.times[i]))
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 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 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 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)