Python SofaPython.Quaternion类代码示例

    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

def run():
    ok=True
    qi= Quaternion.id()
    ok &= EXPECT_VEC_EQ([0,0,0,1],qi)
    q1=[0,0.5,0,1]
    ok &= test_inv(q1)
    return ok

    def setup(self, parentNode, density=2000, param=None, generatedDir = None ):

        # Computation the offset according to the attribute self.transform
        offset = [0,0,0,0,0,0,1]
        offset[:3] = self.transform[:3]
        offset[3:] = Quaternion.from_euler(self.transform[3:6])
        scale3d = self.transform[6:]
        if self.type not in BoneType:
            self.type = "short" # --> to set 1 frame on bone which not have a type

        # Creation of the shearless affine body
        self.body = RigidScale.API.ShearlessAffineBody(parentNode, self.name)

        # Depending on the frame set in the constructor, let decide how the body will be initialized
        if (self.frame == None) or (len(self.frame) < FramePerBoneType[self.type]):
            self.body.setFromMesh(self.filepath, density, offset, scale3d, self.voxelSize, FramePerBoneType[self.type], generatedDir=generatedDir)
            for p in self.body.frame: self.frame.append(p.offset())
        else:
            scale3dList = []
            for i in range(len(self.frame)):
                scale3dList.append(scale3d)
            self.body.setManually(self.filepath, self.frame, self.voxelSize, density=1000, generatedDir=generatedDir)

        # Add of the behavior model, the collision model and the visual model
        localGeneratedDir=None if generatedDir is None else generatedDir+self.name
        self.behavior = self.body.addBehavior(self.elasticity, IntegrationPointPerBoneType[self.type], generatedDir=localGeneratedDir )
        self.collision = self.body.addCollisionMesh(self.filepath, scale3d, offset, generatedDir=localGeneratedDir)
        self.visual = self.collision.addVisualModel()
        return self.body

 def __init__(self, node, filepath, scale3d, offset):
     self.node = node.createChild( "collision" )  # node
     r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi
     self.loader = self.node.createObject("MeshObjLoader", name='loader', filename=filepath, scale3d=concat(scale3d), translation=concat(offset[:3]), rotation=concat(r), triangulate=1 )
     self.topology = self.node.createObject('MeshTopology', name='topology', src="@loader" )
     self.dofs = self.node.createObject('MechanicalObject', name='dofs')
     self.triangles = self.node.createObject('TriangleModel', template='Vec3d', name='model')
     self.mapping = self.node.createObject('RigidMapping', name="mapping")

 def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
     global idxVisualModel
     self.node = node.createChild( "visual"+name_suffix )  # node
     r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi
     self.model = self.node.createObject('VisualModel', name="visual"+str(idxVisualModel), fileMesh=filepath,
                                         scale3d=concat(scale3d), translation=concat(offset[:3]) , rotation=concat(r),
                                         useNormals=False, updateNormals=True)
     self.mapping = self.node.createObject('RigidMapping', name="mapping")
     idxVisualModel+=1

 def __init__(self, node, filepath, scale3d, offset, name_suffix='', generatedDir=None):
     r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
     global idxVisualModel;
     self.node = node.createChild('visual'+name_suffix)  # node
     self.model = self.node.createObject('VisualModel', name='visual'+str(idxVisualModel), fileMesh=filepath, scale3d=concat(scale3d), translation=concat(offset[:3]), rotation=concat(r))
     if generatedDir is None:
         self.mapping = self.node.createObject('LinearMapping', template='Affine,ExtVec3f', name='mapping')
     else:
         serialization.importLinearMapping(self.node, generatedDir+"_visualmapping.json")
     idxVisualModel+=1

 def getWorldInertia(self):
     """ @return inertia with respect to world reference frame
     """
     R = Quaternion.to_matrix(self.inertia_rotation)
     # I in world axis
     I = numpy.dot(R.transpose(), numpy.dot(numpy.diag(self.diagonal_inertia), R))
     # I at world origin, using // axis theorem
     # see http://www.colorado.edu/physics/phys3210/phys3210_sp14/lecnotes.2014-03-07.More_on_Inertia_Tensors.html
     # or https://en.wikipedia.org/wiki/Moment_of_inertia
     a=numpy.array(self.com).reshape(3,1)
     return I + self.mass*(pow(numpy.linalg.norm(self.com),2)*numpy.eye(3) - a*a.transpose())

def decomposeInertia(inertia):
    """ Decompose an inertia matrix into
    - a diagonal inertia
    - the rotation (quaternion) to get to the frame in wich the inertia is diagonal
    """
    U, diagonal_inertia, V = numpy.linalg.svd(inertia)
    # det should be 1->rotation or -1->reflexion
    if numpy.linalg.det(U) < 0 : # reflexion
        # made it a rotation by negating a column
        U[:,0] = -U[:,0]
    inertia_rotation = Quaternion.from_matrix( U )
    return diagonal_inertia, inertia_rotation

 def loadVisualCylinder(self, meshPath, offset = [0,0,0,0,0,0,1], scale=[1,1,1], color=[1,1,1,1],radius=0.01,**kwargs):
     r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi
     self.visual = self.node.createObject("OglCylinderModel", radius=radius, position="@topology.position", edges="@topology.edges" )
     self.normals = self.visual

 def loadVisual(self, meshPath, offset = [0,0,0,0,0,0,1], scale=[1,1,1], color=[1,1,1,1],**kwargs):
     r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi
     self.visual =  self.node.createObject("VisualModel", name="model", filename=meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), color=concat(color), putOnlyTexCoords=True,computeTangents=True,**kwargs)
     # self.visual =  self.node.createObject("VisualModel", name="model", filename=meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), color=concat(color), **kwargs)
     self.visual.setColor(color[0],color[1],color[2],color[3]) # the previous assignement fails when reloading a scene..
     self.normals = self.visual

 def loadMesh(self, meshPath, offset = [0,0,0,0,0,0,1], scale=[1,1,1], triangulate=False):
     r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi
     self.meshLoader = SofaPython.Tools.meshLoader(self.node, meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), triangulate=triangulate)
     self.topology = self.node.createObject("MeshTopology", name="topology", src="@"+self.meshLoader.name )

 def __init__(self):
     self.mass=0.
     self.com=[0.,0.,0.]
     self.diagonal_inertia=[0.,0.,0.]
     self.inertia_rotation=Quaternion.id()

def test_inv(q):
    q_inv=Quaternion.inv(q)
    return EXPECT_VEC_EQ(Quaternion.id(), Quaternion.prod(q,q_inv), "test_inv")

def inv(x):
    res = np.zeros(7)

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

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