C# LinearMath.IndexedQuaternion类代码示例

 public IndexedBasisMatrix(ref IndexedQuaternion q)
 {
     float d = q.LengthSquared();
     Debug.Assert(d != 0.0f);
     float s = 2.0f / d;
     float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
     float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
     float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
     float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
     _el0 = new IndexedVector3(1.0f - (yy + zz), xy - wz, xz + wy);
     _el1 = new IndexedVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
     _el2 = new IndexedVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
 }

	    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;
	    }

    //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
    public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
    {
        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
        frame1._origin = frame1v;

        IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
        IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
        IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
        frame2._origin = frame2v;
        constraint.SetFrames(ref frame1, ref frame2);
        return true;
    }

    public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
    {
        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
        IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
        IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                              _orientation.W);
        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
        mat._origin = vposition;
        collisionObject.SetWorldTransform(mat);

    }

    public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)

    {
        Generic6DofSpringConstraint constrain = null;
        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
        if (body1 != null && body2 != null)
        {
            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
            frame1._origin = frame1v;

            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
            frame2._origin = frame1v;

            constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);

            constrain.CalculateTransforms();
        }

        return new BulletConstraintXNA(constrain);
    }

		public void SetMotorTarget(ref IndexedQuaternion qAinB, float dt) // qAinB is rotation of body A wrt body B.
		{
			// convert target from body to constraint space
            IndexedQuaternion qConstraint = MathUtil.QuaternionInverse(m_rbBFrame.GetRotation())* qAinB * m_rbAFrame.GetRotation();

            qConstraint.Normalize();

			// extract "pure" hinge component
			IndexedVector3 vNoHinge = MathUtil.QuatRotate(ref qConstraint, ref vHinge);
			vNoHinge.Normalize();
			IndexedQuaternion qNoHinge = MathUtil.ShortestArcQuat(ref vHinge, ref vNoHinge);
			IndexedQuaternion qHinge = MathUtil.QuaternionInverse(ref qNoHinge) * qConstraint;
			qHinge.Normalize();

			// compute angular target, clamped to limits
			float targetAngle = MathUtil.QuatAngle(ref qHinge);

			if (targetAngle > MathUtil.SIMD_PI) // long way around. flip quat and recalculate.
			{
				qHinge = -qHinge;
				targetAngle = MathUtil.QuatAngle(ref qHinge);
			}
			if (qHinge.Z < 0)
			{
				targetAngle = -targetAngle;
			}

			SetMotorTarget(targetAngle, dt);

		}

 public static float Dot(IndexedQuaternion q, IndexedQuaternion q2)
 {
     return q.X * q2.X + q.Y * q2.Y + q.Z * q2.Z + q.W * q2.W;
 }

 public static IndexedQuaternion Inverse(IndexedQuaternion q)
 {
     return new IndexedQuaternion(-q.X, -q.Y, -q.Z, q.W);
 }

 internal static bool AlmostEqual(ref IndexedQuaternion v1, ref IndexedQuaternion v2, float nEpsilon)
 {
     return
     (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
     (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
     (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) &&
     (((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon)));
 }

        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 GetRotation(ref IndexedBasisMatrix a, out IndexedQuaternion rot)
 {
     rot = a.GetRotation();
 }

        public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle)
        {
            IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a);
            IndexedQuaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);

            ///floating point inaccuracy can lead to w component > 1..., which breaks 
            dorn.Normalize();
            angle = MathUtil.QuatAngle(ref dorn);
            axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);

            //check for axis length
            float len = axis.LengthSquared();
            if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
            {
                axis = new IndexedVector3(1f, 0, 0);
            }
            else
            {
                axis.Normalize();
            }
        }

 public static void CalculateVelocityQuaternion(ref IndexedVector3 pos0, ref IndexedVector3 pos1, ref IndexedQuaternion orn0, ref IndexedQuaternion orn1, float timeStep, out IndexedVector3 linVel, out IndexedVector3 angVel)
 {
     linVel = (pos1 - pos0) / timeStep;
     if (orn0 != orn1)
     {
         IndexedVector3 axis;
         float angle;
         CalculateDiffAxisAngleQuaternion(ref orn0, ref orn1, out axis, out angle);
         angVel = axis * (angle / timeStep);
     }
     else
     {
         angVel = IndexedVector3.Zero;
     }
 }

//.........这里部分代码省略.........
            }

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
            DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                    if (bestedge == 0)
                    {
                        IndexedVector3 edge = (v0 - v1);
                        isNearEdge = true;

                        if (info.m_edgeV0V1Angle == 0.0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {

                            bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
                            float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
                            IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

                            float NdotA = localContactNormalOnB.Dot(ref nA);
                            float NdotB = localContactNormalOnB.Dot(ref nB);
                            bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

#if DEBUG_INTERNAL_EDGE
                            {

                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 clampedLocalNormal;
                                bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {