C# Quaternion.ToRotationMatrix方法代码示例

        /// <summary>
        /// quternion -> matrix -> quaternion.
        /// </summary>
        public static void Test2()
        {
            using (var writer = new StreamWriter("test-quaternion2.txt"))
            {
                int length = 5;
                for (int angleDegree = 1; angleDegree < 361; angleDegree++)
                {
                    for (int x = -length; x < length; x++)
                    {
                        for (int y = -length; y < length; y++)
                        {
                            for (int z = -length; z < length; z++)
                            {
                                var quaternion = new Quaternion(angleDegree, new vec3(x, y, z));
                                mat3 matrix1 = quaternion.ToRotationMatrix();
                                Quaternion quaternion2 = matrix1.ToQuaternion();
                                writer.WriteLine("====================");
                                writer.WriteLine("{3}° x[{0}] y[{1}] z[{2}]", x, y, z, angleDegree);
                                writer.WriteLine(quaternion);
                                writer.WriteLine("------------");
                                writer.WriteLine(quaternion2);
                                //}
                            }
                        }
                    }
                }

                writer.WriteLine("Test finished.");
            }
        }

        /// <summary>
        /// This test shows that glm.rotate() and Quaternion.ToRotationMatrix() give the same result.
        /// </summary>
        public static void Test()
        {
            using (var writer = new StreamWriter("test-quaternion.txt"))
            {
                int length = 5;
                for (int angleDegree = 1; angleDegree < 361; angleDegree++)
                {
                    for (int x = -length; x < length; x++)
                    {
                        for (int y = -length; y < length; y++)
                        {
                            for (int z = -length; z < length; z++)
                            {
                                var quaternion = new Quaternion(angleDegree, new vec3(x, y, z));
                                mat3 matrix1 = quaternion.ToRotationMatrix();
                                //mat4 tmp = glm.rotate((float)(angleDegree * Math.PI / 180.0f), new vec3(x, y, z));
                                mat4 tmp = glm.rotate(angleDegree, new vec3(x, y, z));
                                mat3 matrix2 = tmp.to_mat3();
                                writer.WriteLine("====================");
                                writer.WriteLine("{3}° x[{0}] y[{1}] z[{2}]", x, y, z, angleDegree);
                                writer.WriteLine(matrix1.ToArray().PrintVectors(3, ",", ";" + Environment.NewLine));
                                writer.WriteLine("------------");
                                writer.WriteLine(matrix2.ToArray().PrintVectors(3, ",", ";" + Environment.NewLine));
                                //}
                            }
                        }
                    }
                }

                writer.WriteLine("Test finished.");
            }
        }

        /// <summary>
        /// Constructor which calculates the Euler Angles from a quaternion.
        /// </summary>
        public Euler(Quaternion oriantation)
        {
            Matrix3 rotMat;
            rotMat = oriantation.ToRotationMatrix();

            // BUGGY METHOD (NaN return in some cases)
            // rotMat.ToEulerAnglesYXZ(out mYaw, out mPitch, out mRoll);

            // WORKAROUND
            bool success;
            Matrix3ToEulerAnglesYXZ(rotMat, out mYaw, out mPitch, out mRoll, out success);

            mChanged = true;
            mCachedQuaternion = Quaternion.IDENTITY;
        }

        public ActorDesc CreateActorDesc(EntityWorldEntity entityNode, Entity entity, Vector3 position, Quaternion orientation, Vector3 scale)
        {
            ActorDesc actorDesc = new ActorDesc();
            actorDesc.GlobalPosition = position;
            actorDesc.GlobalOrientation = orientation.ToRotationMatrix();

            if (entityNode.CollisionMode == CollisionMode.ConvexHull || entityNode.CollisionMode == CollisionMode.TriangleMesh)
            {
                StaticMeshData meshData = new StaticMeshData(entity.GetMesh(), scale);

                if (entityNode.CollisionMode == CollisionMode.TriangleMesh)
                    actorDesc.Shapes.Add(Engine.Physics.CreateTriangleMesh(meshData));
                else
                    actorDesc.Shapes.Add(Engine.Physics.CreateConvexHull(meshData));
            }
            else
            {
                switch (entityNode.CollisionMode)
                {
                    case CollisionMode.BoundingBox:
                        actorDesc.Shapes.Add(new BoxShapeDesc(entity.BoundingBox.HalfSize * scale, entity.BoundingBox.Center * scale));
                        break;
                    case CollisionMode.BoundingSphere:
                        actorDesc.Shapes.Add(new SphereShapeDesc(Engine.MaxAxis(entity.BoundingBox.HalfSize), entity.BoundingBox.Center * scale));
                        break;
                    case CollisionMode.Shapes:
                        foreach (ShapeDesc shapeDesc in entityNode.Shapes)
                            actorDesc.Shapes.Add(shapeDesc);
                        break;
                    default:
                        throw new Exception(entityNode.CollisionMode.ToString() + " not implemented");
                }
            }

            return actorDesc;
        }

        public override void Spawn(Vector3 position, Quaternion orientation)
        {
            // create scene node
            SceneNode sceneNode = this.CreateSceneNode(position, orientation);
            Entity entity = sceneNode.GetAttachedObject(0) as Entity;

            // physics
            BodyDesc bodyDesc = new BodyDesc();
            bodyDesc.LinearVelocity = this.Velocity;

            ActorDesc actorDesc = Engine.Physics.CreateActorDesc(this, entity, position, orientation, this.Scale);
            actorDesc.Density = this.Density;
            actorDesc.Body = bodyDesc;
            actorDesc.GlobalPosition = position;
            actorDesc.GlobalOrientation = orientation.ToRotationMatrix();

            if (this.EnableCCD)
            {
                foreach (ShapeDesc shapeDesc in actorDesc.Shapes)
                    shapeDesc.ShapeFlags = ShapeFlags.DynamicDynamicCCD;
            }

            Actor actor = Engine.Physics.Scene.CreateActor(actorDesc);
            actor.UserData = this;

            ActorNode actorNode = new ActorNode(sceneNode, actor);
            Engine.World.ActorNodes.Add(actorNode);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="ent"></param>
        /// <param name="position"></param>
        /// <param name="orientation"></param>
        /// <param name="scale"></param>
        /// <param name="color"></param>
        public void AddEntity(Entity ent, Vector3 position, Quaternion orientation, Vector3 scale, ColorEx color) {
            Mesh mesh = ent.Mesh;
            if (mesh.SharedVertexData != null)
                throw new Exception("Shared vertex data not allowed");

            //For each subentity
            for (int i = 0; i < ent.SubEntityCount; i++) {
                //get the subentity
                SubEntity subEntity = ent.GetSubEntity(i);
                SubMesh subMesh = subEntity.SubMesh;

                //Generate a format string that uniquely identifies this material & vertex/index format
                if (subMesh.vertexData == null)
                    throw new Exception("Submesh vertex data not found!");

                string formatStr = GetFormatString(subEntity);
                //If a batch using an identical format exists...
                SubBatch batch = null;
                if (!mSubBatches.TryGetValue(formatStr, out batch)) {
                    batch = new SubBatch(this, subEntity);
                    mSubBatches.Add(formatStr, batch);
                }
                //Now add the submesh to the compatible batch
                batch.AddSubEntity(subEntity, position, orientation, scale, color);
            }//end for

            //Update bounding box
            Matrix4 mat = Matrix4.FromMatrix3(orientation.ToRotationMatrix());
            mat.Scale = scale;
            AxisAlignedBox entBounds = ent.BoundingBox;
            entBounds.Transform(mat);
            if (mBoundsUndefinded) {
                mBounds.Minimum = entBounds.Minimum + position;
                mBounds.Maximum = entBounds.Maximum + position;
                mBoundsUndefinded = false;
            }
            else {
                Vector3 min = mBounds.Minimum;
                Vector3 max = mBounds.Maximum;

                min.Floor(entBounds.Minimum + position);
                max.Ceil(entBounds.Maximum + position);
                mBounds.Minimum = min;
                mBounds.Maximum = max;
            }
        }

 /// <summary>
 /// 
 /// </summary>
 /// <param name="ent"></param>
 /// <param name="position"></param>
 /// <param name="rotation"></param>
 /// <param name="scale"></param>
 public virtual void AddEntityToBoundingBox(Entity ent, Vector3 position, Quaternion rotation, Vector3 scale)
 {
     #warning Matrix4 accepts no Quaternation in ctor
     Matrix4 mat = Matrix4.FromMatrix3(rotation.ToRotationMatrix());
     mat.Scale = scale;
     AxisAlignedBox entBounds = ent.BoundingBox;
     Vector3 relPosition = position - mCenterPoint;
     if (mTrueBoundsUndefined)
     {
         mTrueBounds.Minimum = entBounds.Minimum + relPosition;
         mTrueBounds.Maximum = entBounds.Maximum + relPosition;
         mTrueBoundsUndefined = false;
     }
     else
     {
         Vector3 min = mTrueBounds.Minimum;
         Vector3 max = mTrueBounds.Maximum;
         min.Floor(entBounds.Minimum + relPosition);
         max.Floor(entBounds.Maximum + relPosition);
         mTrueBounds.Maximum = max;
         mTrueBounds.Minimum = min;
     }
 }