示例#1
0
	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]))
示例#2
0
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))
示例#3
0
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))
示例#4
0
 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))
示例#5
0
 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))
示例#6
0
 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))
示例#7
0
文件: loader.py 项目: RGrant92/Klampt
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)