C# Body.DirectionToWorld方法代码示例

        private static void LimitRotation(Body body, Transform3D rotate_FromWorld, Transform3D rotate_ToWorld, Viewport3D viewport, List<Visual3D> debugVisuals)
        {
            const double DEADDOT = .33;

            Vector3D z = new Vector3D(0, 0, 1);

            Vector3D reversed = rotate_FromWorld.Transform(z);
            Vector3D current = body.DirectionToWorld(reversed);





            Vector3D whatIsThis = rotate_FromWorld.Transform(current);






            Quaternion diff = Math3D.GetRotation(rotate_FromWorld.Transform(current), reversed);


            Vector3D test = body.DirectionToWorld(z);

            Vector3D cross = Vector3D.CrossProduct(z, -test);
            Vector3D check = Vector3D.CrossProduct(z, reversed);
            double dot = Vector3D.DotProduct(cross, check);
            if (Math.Abs(dot) < DEADDOT)
            {
                //TODO: Fix this properly.  The problem has something to do with loss of accuracy around the X axis, because the ship is already being rotated
                //about that axis.  Is this the definition of gimbal lock?
                //In this case, will probably need to rotate everything be 90 degrees, then rebuild diff - or something?
                diff = Quaternion.Identity;
            }
            else if (dot < -DEADDOT)
            {
                diff = new Quaternion(diff.Axis, -diff.Angle);
            }

            //body.Rotation = body.Rotation.RotateBy(diff);






            //Quaternion diff = Math3D.GetRotation(current, z);     why doesn't this work?!?!?!
            //I wonder if the flaw is in the extension methd:
            //  body.Rotation = body.Rotation.RotateBy(diff);
            //
            //Try multiplying quaternions in a different order to see if the diff between current and z can be made to work

            //https://www.google.com/?gws_rd=ssl#safe=active&q=quaternion+multiplication+order
            //http://answers.unity3d.com/questions/810579/quaternion-multiplication-order.html

            //OR: is body.DirectionToWorld doing more than body.Rotation
            //
            //Use the F keys to get and draw various rotations, to test if there's more than just body.Rotation - or something










            //Vector3D verifyRotation = body.Rotation.GetRotatedVector(reversed);

            ////if(!Math3D.IsNearValue(current.ToUnit(), verifyRotation.ToUnit()))        // too strict
            //if (Vector3D.DotProduct(current.ToUnit(), verifyRotation.ToUnit()) < .99)       // this if statement never hits
            //{
            //    //throw new ApplicationException("AAAAHHHHAAAAA!!!!!!!!!!!!!!!");
            //}



            ////TODO: Come up with a visualization that demonstrates why these are different.  That visualization will illuminate the gimbal lock
            //Quaternion diffProper = Math3D.GetRotation(current, z);
            //Quaternion diffHacked = Math3D.GetRotation(whatIsThis, reversed);


            //Point3D position = body.Position;

            //DrawLine(position, z * 100, Colors.DarkGreen, viewport, debugVisuals);
            //DrawLine(position, current * 100, Colors.LimeGreen, viewport, debugVisuals);
            ////if (!Math3D.IsNearZero(diffProper.Angle))
            //{
            //    DrawLine(position, diffProper.Axis * 100, Colors.PaleGreen, viewport, debugVisuals);
            //}


            //DrawLine(position, reversed * 100, Colors.Maroon, viewport, debugVisuals);
            //DrawLine(position, whatIsThis * 100, Colors.OrangeRed, viewport, debugVisuals);
            ////if (Math.Abs(diffHacked.Angle) > .001)
            //{
            //    DrawLine(position, diffHacked.Axis * 100, Colors.Wheat, viewport, debugVisuals);
            //}
//.........这里部分代码省略.........

        private static void LimitRotation_ONEQUADRANT(Body body, Transform3D rotate_FromWorld, Transform3D rotate_ToWorld, Viewport3D viewport, List<Visual3D> debugVisuals)
        {
            Vector3D z = new Vector3D(0, 0, 1);

            Vector3D reversed = rotate_FromWorld.Transform(z);
            Vector3D current = body.DirectionToWorld(reversed);

            //TODO: This only works for one quadrant

            Quaternion diff = Math3D.GetRotation(current, z);
            diff = new Quaternion(rotate_FromWorld.Transform(diff.Axis), diff.Angle);

            body.Rotation = body.Rotation.RotateBy(diff);







            Point3D position = body.Position;

            //DrawLine(position, z * 100, Colors.DarkOrchid, viewport, debugVisuals);     // won't be visible
            //DrawLine(position, current * 100, Colors.HotPink, viewport, debugVisuals);

            //DrawLine(position, diff.Axis * 100, Colors.Yellow, viewport, debugVisuals);




            //Vector3D angularVelocity = body.AngularVelocity.GetProjectedVector(z);
            Vector3D angularVelocity = body.AngularVelocity;

            DrawLine(position, angularVelocity * 100, Colors.Yellow, viewport, debugVisuals);

            angularVelocity = angularVelocity.GetProjectedVector(z);

            DrawLine(position, angularVelocity * 100, Colors.HotPink, viewport, debugVisuals);

            body.AngularVelocity = angularVelocity;


        }

        private static void LimitLinear(Body body, Transform3D rotate_ToWorld)
        {
            // Position
            Point3D position = body.Position;       //NOTE: Position is at the center of mass

            Point3D centerMassModel = body.CenterOfMass;
            Point3D centerMassWorld = rotate_ToWorld.Transform(centerMassModel);        // position is model coords, but rotated into world coords

            Point3D centerMassActual = body.DirectionToWorld(centerMassModel);

            body.Position = new Point3D(position.X - centerMassActual.X, position.Y - centerMassActual.Y, centerMassWorld.Z);

            // Velocity
            Vector3D velocity = body.Velocity;
            body.Velocity = new Vector3D(velocity.X, velocity.Y, 0);
        }