C# IndexedMatrix.GetRotation方法代码示例

 public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
 {
     EntityProperties ret = new EntityProperties();
     ID = id;
     Position = startTransform._origin;
     Rotation = startTransform.GetRotation();
     return ret;
 }

        public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
        {
            m_xform = worldTrans;
            // Put the new transform into m_properties
            IndexedQuaternion OrientationQuaternion = m_xform.GetRotation();
            IndexedVector3 LinearVelocityVector = Rigidbody.GetLinearVelocity();
            IndexedVector3 AngularVelocityVector = Rigidbody.GetAngularVelocity();
            m_properties.Position = new Vector3(m_xform._origin.X, m_xform._origin.Y, m_xform._origin.Z);
            m_properties.Rotation = new Quaternion(OrientationQuaternion.X, OrientationQuaternion.Y,
                                                   OrientationQuaternion.Z, OrientationQuaternion.W);
            // A problem with stock Bullet is that we don't get an event when an object is deactivated.
            // This means that the last non-zero values for linear and angular velocity
            // are left in the viewer who does dead reconning and the objects look like
            // they float off.
            // BulletSim ships with a patch to Bullet which creates such an event.
            m_properties.Velocity = new Vector3(LinearVelocityVector.X, LinearVelocityVector.Y, LinearVelocityVector.Z);
            m_properties.RotationalVelocity = new Vector3(AngularVelocityVector.X, AngularVelocityVector.Y, AngularVelocityVector.Z);

            if (force

                || !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE)
                || !AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
                // If the Velocity and AngularVelocity are zero, most likely the object has
                //    been deactivated. If they both are zero and they have become zero recently,
                //    make sure a property update is sent so the zeros make it to the viewer.
                || ((m_properties.Velocity == ZeroVect && m_properties.RotationalVelocity == ZeroVect)
                    &&
                    (m_properties.Velocity != m_lastProperties.Velocity ||
                     m_properties.RotationalVelocity != m_lastProperties.RotationalVelocity))
                //	If Velocity and AngularVelocity are non-zero but have changed, send an update.
                || !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
                ||
                !AlmostEqual(ref m_properties.RotationalVelocity, ref m_lastProperties.RotationalVelocity,
                             ANGULARVELOCITY_TOLERANCE)
                )


            {
                // Add this update to the list of updates for this frame.
                m_lastProperties = m_properties;
                if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
                    m_world.UpdatedObjects[m_world.LastEntityProperty++]=(m_properties);

                //(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
            }




        }

 public SimMotionState(BSAPIXNA pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
 {
     IndexedQuaternion OrientationQuaterion = starTransform.GetRotation();
     m_properties = new EntityProperties()
                        {
                            ID = id,
                            Position = new Vector3(starTransform._origin.X, starTransform._origin.Y,starTransform._origin.Z),
                            Rotation = new Quaternion(OrientationQuaterion.X,OrientationQuaterion.Y,OrientationQuaterion.Z,OrientationQuaterion.W)
                        };
     m_lastProperties = new EntityProperties()
     {
         ID = id,
         Position = new Vector3(starTransform._origin.X, starTransform._origin.Y, starTransform._origin.Z),
         Rotation = new Quaternion(OrientationQuaterion.X, OrientationQuaterion.Y, OrientationQuaterion.Z, OrientationQuaterion.W)
     };
     m_world = pWorld;
     m_xform = starTransform;
 }

        public virtual void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
        {
            BulletGlobals.StartProfile("convexSweepTest");
            /// use the broadphase to accelerate the search for objects, based on their aabb
            /// and for each object with ray-aabb overlap, perform an exact ray test
            /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical

            IndexedMatrix convexFromTrans;
            IndexedMatrix convexToTrans;
            convexFromTrans = convexFromWorld;
            convexToTrans = convexToWorld;
            IndexedVector3 castShapeAabbMin;
            IndexedVector3 castShapeAabbMax;
            /* Compute AABB that encompasses angular movement */
            {
                IndexedVector3 linVel;
                IndexedVector3 angVel;
                TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
                IndexedVector3 zeroLinVel = new IndexedVector3();
                IndexedMatrix R = IndexedMatrix.Identity;
                R.SetRotation(convexFromTrans.GetRotation());
                castShape.CalculateTemporalAabb(ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
            }

#if !USE_BRUTEFORCE_RAYBROADPHASE
            SingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
            convexCB.Initialize(castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration);
            IndexedVector3 tempFrom = convexFromTrans._origin;
            IndexedVector3 tempTo = convexToTrans._origin;
            m_broadphasePairCache.RayTest(ref tempFrom, ref tempTo, convexCB, ref castShapeAabbMin, ref castShapeAabbMax);
            convexCB.Cleanup();
            BulletGlobals.SingleSweepCallbackPool.Free(convexCB);
#else
	        /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
	        // do a ray-shape query using convexCaster (CCD)
	        int i;
	        for (i=0;i<m_collisionObjects.Count;i++)
	        {
		        CollisionObject	collisionObject= m_collisionObjects[i];
		        //only perform raycast if filterMask matches
		        if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle())) 
                {
			        //RigidcollisionObject* collisionObject = ctrl.GetRigidcollisionObject();
			        IndexedVector3 collisionObjectAabbMin = new IndexedVector3();
                    IndexedVector3 collisionObjectAabbMax = new IndexedVector3();
			        collisionObject.GetCollisionShape().GetAabb(collisionObject.GetWorldTransform(),ref collisionObjectAabbMin,ref collisionObjectAabbMax);
			        AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
			        float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
			        IndexedVector3 hitNormal = new IndexedVector3();
                    IndexedVector3 fromOrigin = convexFromWorld._origin;
                    IndexedVector3 toOrigin = convexToWorld._origin;
                    if (AabbUtil2.RayAabb(ref fromOrigin, ref toOrigin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, ref hitNormal))
			        {
                        IndexedMatrix trans = collisionObject.GetWorldTransform();
				        ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
					        collisionObject,
						        collisionObject.GetCollisionShape(),
						        ref trans,
						        resultCallback,
						        allowedCcdPenetration);
			        }
		        }
	        }
#endif //USE_BRUTEFORCE_RAYBROADPHASE
            BulletGlobals.StopProfile();
        }

 public SimMotionState(CollisionWorld pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
 {
     m_properties = new EntityProperties()
                        {
                            ID = id,
                            Position = starTransform._origin,
                            Rotation = starTransform.GetRotation()
                        };
     m_lastProperties = new EntityProperties()
     {
         ID = id,
         Position = starTransform._origin,
         Rotation = starTransform.GetRotation()
     };
     m_world = pWorld;
     m_xform = starTransform;
 }

        void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB)
	    {
		    m_separatingNormal = separatingVector;
		    m_separatingDistance = separatingDistance;
    		
		    IndexedVector3 toPosA = transA._origin;
		    IndexedVector3 toPosB = transB._origin;
            IndexedQuaternion toOrnA = transA.GetRotation();
            IndexedQuaternion toOrnB = transB.GetRotation();
		    m_posA = toPosA;
		    m_posB = toPosB;
		    m_ornA = toOrnA;
		    m_ornB = toOrnB;
	    }

        public void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB)
        {
            IndexedVector3 toPosA = transA._origin;
            IndexedVector3 toPosB = transB._origin;
            IndexedQuaternion toOrnA = transA.GetRotation();
            IndexedQuaternion toOrnB = transB.GetRotation();

            if (m_separatingDistance > 0.0f)
            {
                IndexedVector3 linVelA;
                IndexedVector3 angVelA;
                IndexedVector3 linVelB;
                IndexedVector3 angVelB;

                TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA);
                TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB);
                float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
                IndexedVector3 relLinVel = (linVelB - linVelA);
                float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal);
                if (relLinVelocLength < 0f)
                {
                    relLinVelocLength = 0f;
                }

                float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
                m_separatingDistance -= projectedMotion;
            }

            m_posA = toPosA;
            m_posB = toPosB;
            m_ornA = toOrnA;
            m_ornB = toOrnB;
        }

	    public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
	    {
            predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
    //	#define QUATERNION_DERIVATIVE
	    #if QUATERNION_DERIVATIVE
            IndexedVector3 pos;
            IndexedQuaternion predictedOrn;
            IndexedVector3 scale;

            curTrans.Decompose(ref scale, ref predictedOrn, ref pos);


		    predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
		    predictedOrn.Normalize();
        #else
            //Exponential map
		    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

		    IndexedVector3 axis;
		    float	fAngle = angvel.Length(); 
		    //limit the angular motion
		    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
		    {
			    fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
		    }

		    if ( fAngle < 0.001f )
		    {
			    // use Taylor's expansions of sync function
			    axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
		    }
		    else
		    {
			    // sync(fAngle) = sin(c*fAngle)/t
			    axis   = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
		    }
		    IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );

            IndexedQuaternion orn0 = curTrans.GetRotation();

		    IndexedQuaternion predictedOrn = dorn * orn0;
		    predictedOrn.Normalize();
	    #endif

            IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
            predictedTransform._basis = newMatrix._basis;
	    }

        public void CalcAngleInfo2(ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedBasisMatrix invInertiaWorldA, ref IndexedBasisMatrix invInertiaWorldB)
		{
			m_swingCorrection = 0;
			m_twistLimitSign = 0;
			m_solveTwistLimit = false;
			m_solveSwingLimit = false;

			// compute rotation of A wrt B (in constraint space)
            if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
			{	// it is assumed that setMotorTarget() was alredy called 
				// and motor target m_qTarget is within constraint limits
				// TODO : split rotation to pure swing and pure twist
				// compute desired transforms in world
				IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
				IndexedMatrix trA = transA * m_rbAFrame;
				IndexedMatrix trB = transB * m_rbBFrame;
                IndexedMatrix trDeltaAB = trB * trPose * trA.Inverse();
				IndexedQuaternion qDeltaAB = trDeltaAB.GetRotation();
				IndexedVector3 swingAxis = new IndexedVector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z);
                float swingAxisLen2 = swingAxis.LengthSquared();
                if (MathUtil.FuzzyZero(swingAxisLen2))
                {
                    return;
                }

				m_swingAxis = swingAxis;
				m_swingAxis.Normalize();
				m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB);
				if (!MathUtil.FuzzyZero(m_swingCorrection))
				{
					m_solveSwingLimit = true;
				}
				return;
			}


			{

				// compute rotation of A wrt B (in constraint space)
				// Not sure if these need order swapping as well?
                IndexedQuaternion qA = transA.GetRotation() * m_rbAFrame.GetRotation();
                IndexedQuaternion qB = transB.GetRotation() * m_rbBFrame.GetRotation();
                
                IndexedQuaternion qAB = MathUtil.QuaternionInverse(qB) * qA;

				// split rotation into cone and twist
				// (all this is done from B's perspective. Maybe I should be averaging axes...)
				IndexedVector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist);
				vConeNoTwist.Normalize();
				IndexedQuaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist);
				qABCone.Normalize();
				IndexedQuaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB;
				qABTwist.Normalize();

				if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
				{
					float swingAngle = 0f, swingLimit = 0f;
					IndexedVector3 swingAxis = IndexedVector3.Zero;
					ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit);

					if (swingAngle > swingLimit * m_limitSoftness)
					{
						m_solveSwingLimit = true;

						// compute limit ratio: 0->1, where
						// 0 == beginning of soft limit
						// 1 == hard/real limit
						m_swingLimitRatio = 1f;
						if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
						{
							m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
												(swingLimit - (swingLimit * m_limitSoftness));
						}

						// swing correction tries to get back to soft limit
						m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);

						// adjustment of swing axis (based on ellipse normal)
						AdjustSwingAxisToUseEllipseNormal(ref swingAxis);

						// Calculate necessary axis & factors		
						m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis);

						m_twistAxisA = IndexedVector3.Zero;

						m_kSwing = 1f /
							(ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) +
							 ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB));
					}
				}
				else
				{
					// you haven't set any limits;
					// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
					// anyway, we have either hinge or fixed joint

                    IndexedVector3 ivA = transA._basis * m_rbAFrame._basis.GetColumn(0);
                    IndexedVector3 jvA = transA._basis * m_rbAFrame._basis.GetColumn(1);
                    IndexedVector3 kvA = transA._basis * m_rbAFrame._basis.GetColumn(2);
                    IndexedVector3 ivB = transB._basis * m_rbBFrame._basis.GetColumn(0);
//.........这里部分代码省略.........