Python Quaternion.rotate方法代码示例

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
def inv(x):
    res = np.zeros(7)

    res[3:] = quat.conj(x[3:])
    res[:3] = -quat.rotate(res[3:], x[:3])
    
    return res

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
def prod(x, y):
    print x.inv()
    res = np.zeros(7)

    res[:3] = x[:3] + quat.rotate(x[3:], y[:3])
    res[3:] = quat.prod(x[3:], y[3:])

    return res

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
    def onBeginAnimationStep(self, dt):

        # current mu from current plane angle
        currentMu = math.tan( self.currentAngle )
        
        if self.counter < 100 : # does not rotate the plane for 100 time steps
            self.counter += 1
            return 0
            
        # is it a mu we want to test?
        if numpy.allclose( self.muToTest, currentMu, 1e-3, 1e-3 ) :
            
            # at the end of 100 time steps, check if the box was sticking or sliding
            self.counter = 0
            self.muToTest += 0.1
            
            # look at the box velocity along its x-axis
            localbox = Quaternion.rotate(Quaternion.conj( Frame.Frame( shared.plane.position[0] ).rotation ),
                                shared.box.velocity[0][:3])
            vel = localbox[0]

            #print 'plane/ground angle:', self.currentAngle
            #print 'velocity:',vel

            #print shared.box.position[0], shared.box.velocity[0][:3]

            #print vel, currentMu, shared.mu
            
            
            testVel = (vel > 1e-1)            
            if testVel:
                testMu = (currentMu>=shared.mu-1e-2)
            else:
                testMu = (currentMu>=shared.mu)
                
            
           
            
            EXPECT_FALSE( testVel ^ testMu, str(vel)+' '+str(currentMu)+'mu='+str(shared.mu) ) # xor
            
            
            #print testVel, testMu
            #sys.stdout.flush()
        
        # all finished
        if currentMu >= shared.mu + .1:
            self.sendSuccess()
        
        
        # update plane orientation
        self.currentAngle += 0.001
        q = Quaternion.from_euler( [0,0,-self.currentAngle] )
        p = shared.plane.position
        p[0] = [0,0,0,q[3],q[2],q[1],q[0]]
        shared.plane.position = p
               
        return 0

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
	def __mul__(self, other):
		res = Frame()
		res.translation = vec.sum(self.translation,
					  quat.rotate(self.rotation,
						      other.translation))
		res.rotation = quat.prod( self.rotation,
					  other.rotation)

		return res

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
 def apply(self, vec):
     """ apply transformation to vec (a [x,y,z] vector)
     return the result
     """
     return array(quat.rotate(self.rotation, vec) + asarray(self.translation))

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
 def inv(self):
     res = Frame()
     res.rotation = quat.conj( self.rotation )
     res.translation = - quat.rotate(res.rotation, self.translation)
     return res

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import rotate [as 别名]
def createScene(node):

    node.createObject('RequiredPlugin',
                      pluginName = 'Compliant')


    ode = node.createObject('CompliantImplicitSolver')
    num = node.createObject('SequentialSolver')

    # ode.debug = 1
    node.dt = 0.01

    pos = np.zeros(7)
    vel = np.zeros(6)
    force = np.zeros(6)

    alpha = math.pi / 4.0
    
    q = quat.exp([0, 0, alpha])

    pos[:3] = [-0.5, 0, 0]
    pos[3:] = q

    mass = 1.0

    # change this for more fun
    dim = np.array([1, 2, 1])

    dim2 = dim * dim
    
    
    inertia = mass / 12.0 * (dim2[ [1, 2, 0] ] + dim2[ [2, 0, 1] ])
    volume = 1.0
    
    force[3:] = quat.rotate(q, [0, 1, 0])

    scene = node.createChild('scene')
    
    good = scene.createChild('good')
    
    dofs = good.createObject('MechanicalObject',
                             template = 'Rigid',
                             name = 'dofs',
                             position = pos,
                             velocity = vel,
                             showObject = 1)

    good.createObject('RigidMass',
                      template = 'Rigid',
                      name = 'mass',
                      mass = mass,
                      inertia = inertia)

    good.createObject('ConstantForceField',
                      template = 'Rigid',
                      name = 'ff',
                      forces = force)

    bad = scene.createChild('bad')

    pos[:3] = [0.5, 0, 0]
    dofs = bad.createObject('MechanicalObject',
                            template = 'Rigid',
                            name = 'dofs',
                            position = pos,
                            velocity = vel,
                            showObject = 1)

    inertia_matrix = np.diag(inertia)
    
    def cat(x): return ' '.join( map(str, x))

    def print_matrix(x):
        return '[' + ','.join(map(str, x)) + ']'

    
    bad.createObject('UniformMass',
                     template = 'Rigid',
                     name = 'mass',
                     mass = cat([mass, volume, print_matrix(inertia_matrix / mass)]))
    
    bad.createObject('ConstantForceField',
                      template = 'Rigid',
                      name = 'ff',
                      forces = force)

                
    node.gravity = '0 0 0'