Python Quaternion.from_euler方法代码示例

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import from_euler [as 别名]
    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

# 需要导入模块: from SofaPython import Quaternion [as 别名]
# 或者: from SofaPython.Quaternion import from_euler [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 from_euler [as 别名]
def run():

    ok = True

    info = SofaPython.mass.RigidMassInfo()

# testing axis-aligned known geometric shapes
    for m in xrange(len(meshes)):
        mesh = meshes[m]
        mesh_path = path + meshes[m]

        for s in xrange(len(scales)):
            scale = scales[s]

            if mesh=="cylinder.obj" and scale[0]!=scale[1]:
                continue
        
            for d in xrange(len(densities)):
                density=densities[d]

                info.setFromMesh( mesh_path, density, scale )

                error = " ("+meshes[m]+", s="+Tools.cat(scale)+" d="+str(density)+")"

                ok &= EXPECT_TRUE( almostEqualReal(info.mass, masses[m][s][d]), "mass"+error+" "+str(info.mass)+"!="+str(masses[m][s][d]) )
                ok &= EXPECT_TRUE( almostEqualLists(info.com,[x*0.5 for x in scale]), "com"+error+" "+Tools.cat(info.com)+"!="+Tools.cat([x*0.5 for x in scale]) )
                ok &= EXPECT_TRUE( almostEqualLists(info.diagonal_inertia,inertia[m][s][d]), "inertia"+error+" "+str(info.diagonal_inertia)+"!="+str(inertia[m][s][d]) )

# testing diagonal inertia extraction from a rotated cuboid
    mesh = "cube.obj"
    mesh_path = path + mesh
    scale = scales[3]
    density = 1
    theory = sorted(inertia[0][3][0])
    for r in rotations:
        info.setFromMesh( mesh_path, density, scale, r )
        local = sorted(info.diagonal_inertia)
        ok &= EXPECT_TRUE( almostEqualLists(local,theory), "inertia "+str(local)+"!="+str(theory)+" (rotation="+str(r)+")" )

# testing extracted inertia rotation
    mesh = "rotated_cuboid_12_35_-27.obj"
    mesh_path = path + mesh
    density = 1
    info.setFromMesh( mesh_path, density )

    # theoretical results
    scale = [2,3,1]
    mass = density * scale[0]*scale[1]*scale[2]
    inertiat = numpy.empty(3)
    inertiat[0] = 1.0/12.0 * mass * (scale[1]*scale[1]+scale[2]*scale[2]) # x
    inertiat[1] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[2]*scale[2]) # y
    inertiat[2] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[1]*scale[1]) # z

    # used quaternion in mesh

    q = Quaternion.normalized( Quaternion.from_euler( [12*math.pi/180.0, 35*math.pi/180.0, -27*math.pi/180.0] ) )

    # corresponding rotation matrices (ie frame defined by columns)
    mt = Quaternion.to_matrix( q )
    m  = Quaternion.to_matrix( info.inertia_rotation )

    # matching inertia
    idxt = numpy.argsort(inertiat)
    idx  = numpy.argsort(info.diagonal_inertia)

    # checking if each axis/column are parallel (same or opposite for unitary vectors)
    for i in xrange(3):
        ok &= EXPECT_TRUE( almostEqualLists(mt[:,idxt[i]].tolist(),m[:,idx[i]].tolist(),1e-5) or almostEqualLists(mt[:,idxt[i]].tolist(),(-m[:,idx[i]]).tolist(),1e-5), "wrong inertia rotation" )


#    print mt[:,idxt]
#    print m [:,idx ]


    return ok