示例#1
0
 public static float GetRotation(ref Matrix matrix)
 {
     Vector3 position3, scale3;
     Quaternion rotationQ;
     matrix.Decompose(out scale3, out rotationQ, out position3);
     Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
     return (float)Math.Atan2(direction.Y, direction.X);
 }
		/// <summary>
		/// Construct this transform from a matrix
		/// </summary>
		/// <param name="matrix"></param>
		/// <param name="performValidityCheck">When true, the matrix will be checked to make sure it will produce a valid transform</param>
		public Transform(ref Matrix matrix, bool performValidityCheck)
		{
			if (performValidityCheck)
			{
				//validate the matrix is not sheered or inverted on a single axis.
				Vector3 x = new Vector3(matrix.M11, matrix.M12, matrix.M13);
				Vector3 y = new Vector3(matrix.M21, matrix.M22, matrix.M23);
				Vector3 z = new Vector3(matrix.M31, matrix.M32, matrix.M33);

				float xl = x.Length();
				float yl = y.Length();
				float zl = z.Length();

				float maxl = Math.Max(xl, Math.Max(yl, zl));
				float minl = Math.Min(xl, Math.Min(yl, zl));

				if ((maxl - minl) > maxl / 10)
					throw new ArgumentException("The input matrix is not uniformly scaled");

				if (xl > 0.000001f) x /= xl;
				if (yl > 0.000001f) y /= yl;
				if (zl > 0.000001f) z /= zl;

				Vector3 zc = Vector3.Cross(x, y);
				Vector3 yc = Vector3.Cross(z, x);
				Vector3 xc = Vector3.Cross(y, zc);

				if (Vector3.Dot(x, xc) < 0.975f || Vector3.Dot(z, zc) * Vector3.Dot(y, yc) < 0.95f)
					throw new ArgumentException("The input matrix is skewed, sheered or non uniformly scaled");
			}

			Vector3 scale;
			matrix.Decompose(out scale, out Rotation, out Translation);

			//if one or two components are negative, then the Decompose messed up.
			if (scale.X * scale.Y * scale.Z < 0)
			{
				Matrix copy = matrix;
				copy.M11 = -copy.M11;
				copy.M12 = -copy.M12;
				copy.M13 = -copy.M13;
				copy.M21 = -copy.M21;
				copy.M22 = -copy.M22;
				copy.M23 = -copy.M23;
				copy.M31 = -copy.M31;
				copy.M32 = -copy.M32;
				copy.M33 = -copy.M33;
				copy.Decompose(out scale, out Rotation, out Translation);
				scale = -scale;
			}

			this.Scale = Math.Min(Math.Min(scale.X, scale.Y), scale.Z);

			if (Scale > 0.999f && Scale < 1.001f)
				Scale = 1;

			this.Rotation.Normalize();
		}
示例#3
0
 public static void DecomposeMatrix2D(ref Matrix matrix, out Vector2 position, out float rotation, out Vector2 scale)
 {
     Vector3 position3, scale3;
     Quaternion rotationQ;
     matrix.Decompose(out scale3, out rotationQ, out position3);
     Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
     rotation = (float)Math.Atan2(direction.Y, direction.X);
     position = new Vector2(position3.X, position3.Y);
     scale = new Vector2(scale3.X, scale3.Y);
 }
示例#4
0
 public virtual void SetTransform(Matrix m)
 {
     Vector3 pos;
     Quaternion quat;
     Vector3 scale;
     m.Decompose(out scale,out quat,out pos);
     Vector3 rot = MathUtils.QuaternionToEuler(quat);
     transform.Set(new Vector2(pos.X,pos.Y), (float)rot.Z);
     SetScale(new Vector2(scale.X,scale.Y));
 }
		/// <summary>
		/// Construct this transform from a matrix
		/// </summary>
		/// <param name="matrix"></param>
		public Transform(ref Matrix matrix)
		{
			Vector3 scale;
			matrix.Decompose(out scale, out Rotation, out Translation);

			this.Scale = Math.Min(Math.Min(scale.X, scale.Y), scale.Z);

			if (Scale > 0.9999f && Scale < 1.0001f)
				Scale = 1;
		}
示例#6
0
        public void SetTransform(ref Matrix transform)
        {
            transform.Decompose(out scale, out rotation, out translation);

            if (scale.X > 0.9999f && scale.X <= 1.0001f)
                scale.X = 1;
            if (scale.Y > 0.9999f && scale.Y <= 1.0001f)
                scale.Y = 1;
            if (scale.Z > 0.9999f && scale.Z <= 1.0001f)
                scale.Z = 1;
        }
示例#7
0
        // Converts a Rotation Matrix to a quaternion, then into a Vector3 containing
        // Euler angles (X: Pitch, Y: Yaw, Z: Roll)
        public Vector3 MatrixToEulerAngleVector3(Matrix Rotation)
        {
            Vector3 translation, scale;
            Quaternion rotation;

            Rotation.Decompose(out scale, out rotation, out translation);

            Vector3 eulerVec = QuaternionToEulerAngleVector3(rotation);

            return eulerVec;
        }
示例#8
0
        /// <summary>
        /// 指定された行列から生成する
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static SQTTransformContent FromMatrix(Matrix matrix)
        {
            // 行列の分解
            Quaternion rotation;
            Vector3 translation;
            Vector3 scale;
            matrix.Decompose(out scale, out rotation, out translation);

            
            return new SQTTransformContent(scale, rotation, translation);
        }
        /// <summary>
        /// Creates a new keyframe from given time and transformation matrix.
        /// Since internally the joint transformation is stored in form of its
        /// scale, rotation and translation components the matrix has to be 
        /// decomposed within this constructor.
        /// </summary>
        /// <param name="time">Time of keyframe</param>
        /// <param name="transform">Transformation</param>
        public JointAnimationKeyFrame(float time, Matrix transform)
        {
            _time = time;

            if (!transform.Decompose(out _scale, out _rotation, out _translation))
            {
                throw new ApplicationException("Could not decompose transformation matrix");
            }

            _transform = transform;
        }
        public static BoundingSphere Transform(this BoundingSphere source, Matrix transformation)
        {
            Vector3 translation;
            Vector3 scaling;
            Quaternion rotation;
            transformation.Decompose(out scaling, out rotation, out translation);

            var transformedSphereRadius = source.Radius * new[] { scaling.X, scaling.Y, scaling.Z }.Max();
            var transformedSphereCenter = Vector3.Transform(source.Center, transformation);

            return new BoundingSphere(transformedSphereCenter, transformedSphereRadius);
        }
示例#11
0
文件: Workarea.cs 项目: DagonGD/Game3
        public static string GetInfo(Matrix matrix)
        {
            Vector3 scales;
            Quaternion quaternion;
            Vector3 translation;
            matrix.Decompose(out scales, out quaternion, out translation);

            return string.Format("Scales:{0}; Angles:{1}; Trans:{2}", scales,
                //GetAngles(matrix),
                QuaternionToEuler2(quaternion),
                translation);
        }
        public void DrawShield(Matrix world)
        {
            Vector3 scale, translation;
            Quaternion rotation;

            //should actually be up...
            Vector3 side = world.Right;
            Vector3 forward = world.Up;

            world.Decompose(out scale, out rotation, out translation);

            scale.Z *= .8f;
            scale *= 1.5f;
            // scale.X *= 3f;
            //translation -= world.Right;

            rotation = Quaternion.Concatenate(rotation, Quaternion.CreateFromAxisAngle(world.Right, MathHelper.ToRadians(90)));

            Matrix.CreateFromQuaternion(ref rotation, out world);
            world = Matrix.CreateScale(scale) * world;
            world *= Matrix.CreateTranslation(translation);

            // world = Matrix.CreateScale(scale*2) * Matrix.CreateFromQuaternion(rotation) * Matrix.CreateRotationZ((float)(Math.PI / 2)) * Matrix.CreateTranslation(translation);

            Matrix[] ModelTransforms = new Matrix[shieldModel.Bones.Count];
            shieldModel.CopyAbsoluteBoneTransformsTo(ModelTransforms);

            foreach (ModelMesh mesh in shieldModel.Meshes)
            {
                foreach (Effect currentEffect in mesh.Effects)
                {
                    currentEffect.CurrentTechnique = currentEffect.Techniques["Colored"];
                    currentEffect.Parameters["xWorld"].SetValue(ModelTransforms[mesh.ParentBone.Index] * world);
                    currentEffect.Parameters["xView"].SetValue(viewMatrix);
                    currentEffect.Parameters["xProjection"].SetValue(projectionMatrix);

                    Vector3 lightDirection = new Vector3(3, -1, 0);
                    lightDirection.Normalize();

                    currentEffect.Parameters["xLightDirection"].SetValue(lightDirection);
                    currentEffect.Parameters["xShieldP1"].SetValue(P1shieldDirection);
                    currentEffect.Parameters["xShieldP2"].SetValue(P2shieldDirection);
                    currentEffect.Parameters["xShipSide"].SetValue(side);
                    currentEffect.Parameters["xShipForward"].SetValue(forward);
                    //   currentEffect.Parameters["xTexture"].SetValue(texture);

                }
                mesh.Draw();
            }
        }
示例#13
0
        public Transform(Matrix matrix)
        {
            matrix.Decompose(out scale, out rotation, out translation);

            if (scale.X > 0.9999f && scale.X <= 1.0001f)
                scale.X = 1;
            if (scale.Y > 0.9999f && scale.Y <= 1.0001f)
                scale.Y = 1;
            if (scale.Z > 0.9999f && scale.Z <= 1.0001f)
                scale.Z = 1;

            #if DEBUG
            this.Validate();
            #endif
        }
示例#14
0
        public static BoundingBox TransformBox(BoundingBox aabb, Matrix transform)
        {
            Vector3 scale;
            Quaternion rotation;
            Vector3 translation;

            transform.Decompose(out scale, out rotation, out translation);

            if (rotation != Quaternion.Identity)
                throw new ArgumentException("Rotation of AABBs is not supported.");

            Vector3 center = 0.5f * (aabb.Max + aabb.Min) + translation;
            Vector3 extent = 0.5f * (aabb.Max - aabb.Min) * scale;

            //return new BoundingBox(center - extent, center + extent);
            return BoundingBox.CreateFromPoints(new Vector3[] { center - extent, center + extent });
        }
示例#15
0
        /// <summary>
        /// 指定された行列から生成する
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static QuatTransform FromMatrix( Matrix matrix )
        {
            // 行列の分解
            Quaternion rotation;
            Vector3 translation;
            Vector3 scale;
            matrix.Decompose( out scale, out rotation, out translation );

            // 一意のスケールか?
            if ( !CloseEnough( scale.X, scale.Y ) || !CloseEnough( scale.X, scale.Z ) )
            {
                throw new InvalidOperationException(
                    "一意のスケール(X,Y,Zが同じスケール値)ではありません" );
            }

            if ( !CloseEnough( scale.X, 1.0f ) )
                throw new InvalidOperationException( "スケール値が1以外です" );

            return new QuatTransform( rotation, translation );
        }
示例#16
0
        public static BoundingSphere TransformBoundingSphere(BoundingSphere originalBoundingSphere, Matrix transformationMatrix)
        {
            Vector3 trans;
            Vector3 scaling;
            Quaternion rot;
            transformationMatrix.Decompose(out scaling, out rot, out trans);

            float maxScale = scaling.X;
            if (maxScale < scaling.Y)
                maxScale = scaling.Y;
            if (maxScale < scaling.Z)
                maxScale = scaling.Z;

            float transformedSphereRadius = originalBoundingSphere.Radius * maxScale;
            Vector3 transformedSphereCenter = Vector3.Transform(originalBoundingSphere.Center, transformationMatrix);

            BoundingSphere transformedBoundingSphere = new BoundingSphere(transformedSphereCenter, transformedSphereRadius);

            return transformedBoundingSphere;
        }
        /// <summary>
        /// DecomposeMatRot decomposes a matrix into its component parts and return the quaternion rotation.
        /// </summary>
        /// <param name="inMat">Matrix to decompose.</param>
        /// <returns>Returns a quaternion rotation from the Matrix.</returns>
        public static Quaternion DecomposeMatRot(Matrix inMat)
        {
            Quaternion rot;
            Vector3 scale;
            Vector3 trans;
            inMat.Decompose(out scale, out rot, out trans);

            return rot;
        }
示例#18
0
        public void GetSRT(Microsoft.Xna.Framework.Matrix m)
        {
            Microsoft.Xna.Framework.Vector3    scale     = Microsoft.Xna.Framework.Vector3.Zero;
            Microsoft.Xna.Framework.Vector3    translate = Microsoft.Xna.Framework.Vector3.Zero;
            Microsoft.Xna.Framework.Quaternion q         = Microsoft.Xna.Framework.Quaternion.Identity;


            m.Decompose(out scale, out q, out translate);

            this.tX = translate.X;
            this.tY = translate.Y;
            this.tZ = translate.Z;

            this.sX = scale.X;
            this.sY = scale.Y;
            this.sZ = scale.Z;

            float Singularity = 0.499f;

            float ww               = q.W * q.W;
            float xx               = q.X * q.X;
            float yy               = q.Y * q.Y;
            float zz               = q.Z * q.Z;
            float lengthSqd        = xx + yy + zz + ww;
            float singularityTest  = q.Y * q.W - q.X * q.Z;
            float singularityValue = Singularity * lengthSqd;

            if (singularityTest > singularityValue)
            {
                this.rX = (-2f * (float)Math.Atan2(q.Z, q.W));
                this.rY = (90.0f) * (float)(Math.PI / 180);
                this.rZ = (0f) * (float)(Math.PI / 180);
            }
            else
            {
                if (singularityTest < -singularityValue)
                {
                    this.rX = (2 * (float)Math.Atan2(q.Z, q.W));
                    this.rY = (-90.0f) * (float)(Math.PI / 180);
                    this.rZ = (0f) * (float)(Math.PI / 180);
                }
                else
                {
                    this.rX = ((float)Math.Atan2(2.0f * (q.Y * q.Z + q.X * q.W), 1.0f - 2.0f * (xx + yy)));
                    this.rY = ((float)Math.Asin(2.0f * singularityTest / lengthSqd));
                    this.rZ = ((float)Math.Atan2(2.0f * (q.X * q.Y + q.Z * q.W), 1.0f - 2.0f * (yy + zz)));
                }
            }

            while (this.rX > Math.PI)
            {
                this.rX -= (float)(2 * Math.PI);
            }
            while (this.rX < -Math.PI)
            {
                this.rX += (float)(2 * Math.PI);
            }
            while (this.rY > Math.PI)
            {
                this.rY -= (float)(2 * Math.PI);
            }
            while (this.rY < -Math.PI)
            {
                this.rY += (float)(2 * Math.PI);
            }
            while (this.rZ > Math.PI)
            {
                this.rZ -= (float)(2 * Math.PI);
            }
            while (this.rZ < -Math.PI)
            {
                this.rZ += (float)(2 * Math.PI);
            }
        }
示例#19
0
        /// <summary>
        /// Decomposes transform matrix to position, rotation and scale
        /// </summary>
        /// <param name="matrix">Matrix to decompose</param>
        /// <param name="position">Resulting position</param>
        /// <param name="rotation">Resulting rotation in radians</param>
        /// <param name="scale">Resulting scale</param>
        /// <returns>true if decomposition is possible, false otherwise</returns>
        protected bool DecomposeMatrix(ref Matrix matrix, out Vector2 position, out float rotation, out Vector2 scale)
        {
            Vector3 position3, scale3;
            Quaternion rotationQ;
            position = Vector2.Zero; scale = Vector2.One; rotation = 0;

            if (!matrix.Decompose(out scale3, out rotationQ, out position3))
            {
                return false;
            }

            Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
            rotation = (float)Math.Atan2(direction.Y, direction.X);
            position = new Vector2(position3.X, position3.Y);
            scale = new Vector2(scale3.X, scale3.Y);

            return true;
        }
示例#20
0
        private Matrix GetAverage(Matrix original)
        {
            List<Matrix> transforms = new List<Matrix>();
            foreach (RelativeMarker marker in supportingMarkers)
            {
                if (marker.Node.MarkerFound && marker.Initialized)
                {
                    transforms.Add(Matrix.Multiply(marker.RelativeTransform,
                        marker.Node.WorldTransformation));
                }
            }

            if (transforms.Count > 0)
            {
                Vector3 origPos, temp;
                Quaternion origRot;
                original.Decompose(out temp, out origRot, out origPos);

                Vector3 pos;
                Quaternion rot;
                foreach (Matrix mat in transforms)
                {
                    mat.Decompose(out temp, out rot, out pos);
                    origPos += pos;
                    Quaternion.Lerp(ref origRot, ref rot, 1.0f / (1 + transforms.Count), out origRot);
                }

                origRot.Normalize();
                origPos *= 1.0f / (1 + transforms.Count);

                Matrix result = Matrix.CreateFromQuaternion(origRot);
                result.Translation = origPos;

                return result;
            }
            else
                return original;
        }
        //----------------------------------------------------------------------//
        // 関数名	TransformRootBone											//
        //    Function name TransformRootBone
        // 機能		ルートとなる関節に合わせてモデルのボーン情報を生成する			//
        //    Create a bone model information in accordance with the joint to become the root function
        // 引数		スケルトンデータ、ボーンデータ									//
        //    Argument data skeleton, bone data
        // 戻り値	なし															//
        //    No return value
        //----------------------------------------------------------------------//
        private void TransformRootBone(Skeleton skeleton, ref Matrix boneTransform)
        {
            Vector3 s, t;
            Quaternion r;
            if (boneTransform.Decompose(out s, out r, out t))
            {
                // I get the coordinates of the joint on the left and right sides of the origin
                var leftPosition = ConvertJointPosition(skeleton, JointType.HipLeft);
                var rightPosition = ConvertJointPosition(skeleton, JointType.HipRight);

                // I get the rotation axis and angle to rotate the bone from the difference of the coordinates
                var direction = Vector3.Normalize(leftPosition - rightPosition);
                var angle = (float)Math.Acos(Vector3.Dot(Vector3.Right, direction));
                var axis = Vector3.Normalize(Vector3.Cross(Vector3.Right, direction));

                // I want to convert to the amount of movement of the model the amount of movement of the origin
                t = ConvertJointPosition(skeleton, JointType.HipCenter) * 1;

                // Set the reference argument by generating bone information
                boneTransform = Matrix.CreateScale(s) * Matrix.CreateFromQuaternion(r) * Matrix.CreateFromAxisAngle(axis, angle) * Matrix.CreateTranslation(t);
            }
        }
        //------------------------------------------------------------------//
        // 関数名	TransformNodeBone										//
        //    Function name TransformNodeBone
        // 機能		ノードとなる関節に合わせてモデルのボーン情報を生成			//
        //    Create a bone model information in accordance with the joint that it is functional nodes
        // 引数		スケルトンデータ											//
        //    Argument skeleton data
        //            始点の関節												//
        //    Joint of the starting point
        //            終点の関節												//
        //    End point of the point
        //            ボーン情報												//
        //    Bone Information
        // 戻り値	なし														//
        //    No return value
        //------------------------------------------------------------------//
        private void TransformNodeBone( Skeleton skeleton, 
										JointType beginJointType, 
										JointType endJointType, 
										ref Matrix boneTransform)
        {
            Vector3 scale, translation;
            Quaternion rotation;

            // Scale, rotation, broken down into the bone matrix position
            // Decompose the function matrix is Decompose ()
            if (boneTransform.Decompose(out scale, out rotation, out translation))
            {
                // I get the coordinates of joints and end joints of the start point
                var beginPosition = ConvertJointPosition(skeleton, beginJointType);
                var endPosition = ConvertJointPosition(skeleton, endJointType);

                // I get the rotation axis and angle to rotate the bone from the difference of the coordinates
                var direction = Vector3.Normalize(endPosition - beginPosition);
                var angle = (float)Math.Acos(Vector3.Dot(Vector3.Down, direction));
                var axis = Vector3.Normalize(Vector3.Cross(Vector3.Down, direction));

                // I cut off the rotation of the parent bone
                rotation = Quaternion.Identity;

                // Set the reference argument by generating bone information
                boneTransform = Matrix.CreateScale(scale)
                                    * Matrix.CreateFromQuaternion(rotation)
                                        * Matrix.CreateFromAxisAngle(axis, angle)
                                            * Matrix.CreateTranslation(translation);
            }
        }
示例#23
0
 /// <summary>
 /// Updates this node's gloabl matrix based on the parent's global matrix, which
 /// is passed in and this node's local matrix
 /// </summary>
 /// <param name="parentMatrix"></param>
 public virtual void UpdateGlobalMatrix(Matrix parentMatrix)
 {
     globalMatrix = localMatrix * parentMatrix;
     inverseGlobalMatrix = Matrix.Invert(globalMatrix);
     Quaternion quat;
     Vector3 pos;
     Vector3 scl;
     globalMatrix.Decompose(out scl, out quat, out pos);
     globalPosition = new Vector2(pos.X, pos.Y);
     globalRotation = MathUtils.QuaternionToEuler(quat).Z;
     globalScale = new Vector2(scl.X, scl.Y);
     UpdateChildMatrices();
 }
        public virtual void Update(RenderContext renderContext)
        {
            WorldMatrix = Matrix.CreateFromQuaternion(LocalRotation) *
                          Matrix.CreateScale(LocalScale) *
                          Matrix.CreateTranslation(LocalPosition);

            if (Parent != null)
            {
                WorldMatrix = Matrix.Multiply(WorldMatrix, Parent.WorldMatrix);

                Vector3 scale, position;
                Quaternion rotation;

                if (!WorldMatrix.Decompose(out scale, out rotation, out position))
                {
                    System.Diagnostics.Debug.WriteLine("Object3D Decompose World Matrix FAILED!");
                }

                WorldPosition = position;
                WorldScale = scale;
                WorldRotation = rotation;
            }
            else
            {
                WorldPosition = LocalPosition;
                WorldScale = LocalScale;
                WorldRotation = LocalRotation;
            }

            Children.ForEach(child => child.Update(renderContext));

            if (_relativeBoundingBox.HasValue)
                BoundingBox = _relativeBoundingBox.Value.Update(WorldMatrix);
        }
示例#25
0
 public static void decomposeMatrix(ref Matrix matrix, out Vector2 position, out Vector2 scale)
 {
     matrix.Decompose(out scale3, out Node.RotationQuaternoin, out position3);
     position.X = position3.X;
     position.Y = position3.Y;
     scale.X = scale3.X;
     scale.Y = scale3.Y;
 }
示例#26
0
        public void SetWorldMatrix(Matrix V)
        {
            fWorldMatrix = V;

              Vector3 Scale;
              Vector3 Translation;
              fWorldMatrix.Decompose(out Scale, out fOrientation, out Translation);

              fOrientation = Quaternion.Inverse(fOrientation);

              // transform the local reference vectors to get the world vectors
              Vector3.Transform(ref fForwardLocal, ref fOrientation, out fForward);
              Vector3.Transform(ref fRightLocal, ref fOrientation, out fRight);
              Vector3.Transform(ref fUpLocal, ref fOrientation, out fUp);
        }
        /// <summary>
        /// Method to transform a bounding box's corners into world space
        /// </summary>
        /// <param name="input">untransformed bounding box</param>
        /// <param name="world">world matrix</param>
        /// <returns>transformed bounding box</returns>
        public static BoundingSphere TransformBox(BoundingSphere input, Matrix world)
        {
            Vector3 scale = Vector3.Zero;
            Vector3 translate = Vector3.Zero;
            Quaternion rotation = Quaternion.Identity;
            world.Decompose(out scale, out rotation, out translate);

            float dist = Math.Max(input.Radius*scale.X,
                Math.Max(input.Radius * scale.Y,
                input.Radius*scale.Z));
            return new BoundingSphere(Vector3.Transform(input.Center,world),
                dist);
        }
示例#28
0
		public void Decompose()
		{
			Matrix m01 = new Matrix(1f, 1f, 1f, 1f, -0.5f, 2.25f, 3.5f, 2f, 1f, -0.5f, -2f, 3.75f, 0, 3.5f, 2f, 1f);
			Vector3 s01 = new Vector3();
			Vector3 t01 = new Vector3();
			Quaternion q01 = new Quaternion();
			bool d01 = m01.Decompose(out s01, out q01, out t01);
			Vector3 s02 = new Vector3(1.732051f, -4.190763f, 2.291288f);
			Vector3 t02 = new Vector3(0f, 3.5f, 2f);
			Quaternion q02 = new Quaternion(0f, 0f, 0f, 1f);
			bool d02 = false;

			Matrix tmp01 = Matrix.CreateFromQuaternion(new Quaternion(3f, 0, 0, -4f));
			Matrix tmp02 = Matrix.CreateTranslation(5.25f, 3.336f, -2.95f);
			Matrix tmp03 = Matrix.CreateScale(2.5f);
			Matrix m11 = Matrix.Multiply(tmp01, tmp02);
			m11 = Matrix.Multiply(m11, tmp03);
			Vector3 s11 = new Vector3();
			Vector3 t11 = new Vector3();
			Quaternion q11 = new Quaternion();
			bool d11 = m11.Decompose(out s11, out q11, out t11);
			Vector3 s12 = new Vector3(2.5f, 73.52721f, 73.52721f);
			Vector3 t12 = new Vector3(13.125f, 8.34f, -7.375f);
			Quaternion q12 = new Quaternion(0.8882616f, 0f,0f, -0.4593379f);
			bool d12 = true;

			Assert.AreEqual(TestHelper.Approximate(s02), TestHelper.Approximate(s01), "#1");
			Assert.AreEqual(TestHelper.Approximate(t02), TestHelper.Approximate(t01), "#2");
			Assert.AreEqual(TestHelper.Approximate(q02), TestHelper.Approximate(q01), "#3");
			Assert.AreEqual(d02, d01, "#4");
			Assert.AreEqual(TestHelper.Approximate(s12), TestHelper.Approximate(s11), "#5");
			Assert.AreEqual(TestHelper.Approximate(t12), TestHelper.Approximate(t11), "#6");
			Assert.AreEqual(TestHelper.Approximate(q12), TestHelper.Approximate(q11), "#7");
			Assert.AreEqual(d12, d11, "#8");						
		}
示例#29
0
        public void ModifyPhysicsObject(IPhysicsObject physObj, Matrix newTransform)
        {
            if (!objectIDs.ContainsKey(physObj))
                return;

            IntPtr body = objectIDs[physObj];

            Quaternion rot;
            Vector3 trans;
            Vector3 scale;
            newTransform.Decompose(out scale, out rot, out trans);

            Matrix startTransform = Matrix.CreateFromQuaternion(rot);
            if (physObj.Shape == ShapeType.Capsule ||
                physObj.Shape == ShapeType.Cone ||
                physObj.Shape == ShapeType.Cylinder ||
                physObj.Shape == ShapeType.ChamferCylinder)
                startTransform *= Matrix.CreateRotationZ(MathHelper.PiOver2);
            startTransform.Translation = trans;

            // if none of the physics properties are modified, then we only need to set the transform
            if (!physObj.Modified && scaleTable[body].Equals(scale))
            {
                SetTransform(physObj, newTransform);
                return;
            }

            if (!scaleTable[body].Equals(scale) || physObj.ShapeModified)
            {
                IntPtr collision = GetNewtonCollision(physObj, scale);

                Newton.NewtonBodySetCollision(body, collision);
                Newton.NewtonReleaseCollision(nWorld, collision);

                scaleTable.Remove(body);
                scaleTable.Add(body, scale);

                physObj.ShapeModified = false;
            }

            // rigidbody is dynamic if and only if mass is non zero, otherwise static
            bool isDynamic = (physObj.Mass != 0.0f && physObj.Interactable);

            physObj.PhysicsWorldTransform = newTransform;

            // set the matrix for the rigid body
            Newton.NewtonBodySetMatrix(body, MatrixHelper.ToFloats(startTransform));

            if (isDynamic)
            {
                // set the transform call back function
                // if the SetTransformCallback is specified by the programmer for this specific
                // physics object, then use the specified one; otherwise, use the default
                // SetTransformCallback function
                if(setTransformMap.ContainsKey(physObj))
                    Newton.NewtonBodySetTransformCallback(body, setTransformMap[physObj]);
                else
                    Newton.NewtonBodySetTransformCallback(body, transformCallback);

                // set the force and torque call back function
                // if the ApplyForceAndTorqueCallback is specified by the programmer for this specific
                // physics object, then use the specified one; otherwise, use the default
                // ApplyForceAndTorqueCallback function
                if(applyForceMap.ContainsKey(physObj))
                    Newton.NewtonBodySetForceAndTorqueCallback(body, applyForceMap[physObj]);
                else
                    Newton.NewtonBodySetForceAndTorqueCallback(body, applyForceAndTorqueCallback);

                // set the mass matrix and moment of inertia
                Vector3 momentOfInertia = physObj.MomentOfInertia;
                if (momentOfInertia.Equals(Vector3.Zero))
                {
                    IntPtr collision = Newton.NewtonBodyGetCollision(body);
                    momentOfInertia = GetMomentOfInertia(physObj, scale, collision);
                }
                // Swap the axis values since Newton's capsule, cone, and cylinder primitives are oriented 
                // along X axis while our primitives are oriented along Y axis
                else if (physObj.Shape == ShapeType.Capsule ||
                    physObj.Shape == ShapeType.Cone ||
                    physObj.Shape == ShapeType.Cylinder ||
                    physObj.Shape == ShapeType.ChamferCylinder)
                {
                    float tmp = momentOfInertia.Y;
                    momentOfInertia.Y = -momentOfInertia.X;
                    momentOfInertia.X = tmp;
                }
                Newton.NewtonBodySetMassMatrix(body, physObj.Mass, momentOfInertia.X,
                    momentOfInertia.Y, momentOfInertia.Z);

                // set the center of mass if not new Vector3()
                if (!physObj.CenterOfMass.Equals(Vector3.Zero))
                {
                    float[] centerOfMass = {physObj.CenterOfMass.X, physObj.CenterOfMass.Y,
                                               physObj.CenterOfMass.Z};
                    // Swap the axis values since Newton's capsule, cone, and cylinder primitives are oriented 
                    // along X axis while our primitives are oriented along Y axis
                    if (physObj.Shape == ShapeType.Capsule ||
                        physObj.Shape == ShapeType.Cone ||
                        physObj.Shape == ShapeType.Cylinder ||
                        physObj.Shape == ShapeType.ChamferCylinder)
                    {
                        centerOfMass[0] = physObj.CenterOfMass.Y;
                        centerOfMass[1] = -physObj.CenterOfMass.X;
                    }
                    Newton.NewtonBodySetCentreOfMass(body, centerOfMass);
                }

                // set the initial force and torque
                Newton.NewtonBodySetVelocity(body, Vector3Helper.ToFloats(physObj.InitialLinearVelocity));
                Newton.NewtonBodySetOmega(body, Vector3Helper.ToFloats(physObj.InitialAngularVelocity));

                // set the damping values
                if (physObj.LinearDamping >= 0)
                    Newton.NewtonBodySetLinearDamping(body, physObj.LinearDamping);

                if (physObj.AngularDamping != -Vector3.One)
                {
                    float[] angularDamping = {physObj.AngularDamping.X, physObj.AngularDamping.Y,
                        physObj.AngularDamping.Z};
                    Newton.NewtonBodySetAngularDamping(body, angularDamping);
                }

                if (physObj.NeverDeactivate)
                    Newton.NewtonBodySetAutoFreeze(body, 0);
                else
                    Newton.NewtonBodySetAutoFreeze(body, 1);
            }

            // Apply extra settings for vehicle joint
            if (physObj is NewtonVehicle)
            {
                NewtonVehicle vehicle = (NewtonVehicle)physObj;
                float[] upDir = Vector3Helper.ToFloats(startTransform.Up);
                vehicle.Joint = Newton.NewtonConstraintCreateVehicle(nWorld, upDir, body);
                if (vehicle.TransformCallback == null)
                    throw new GoblinException("You need to set the TransformCallback for NewtonVehicle");
                if (vehicle.ForceCallback == null)
                    throw new GoblinException("You need to set the ForceCallback for NewtonVehicle");
                if (vehicle.TireUpdateCallback == null)
                    throw new GoblinException("You need to set the TireUpdateCallback for NewtonVehicle");

                Newton.NewtonBodySetTransformCallback(body, vehicle.TransformCallback);
                Newton.NewtonBodySetForceAndTorqueCallback(body, vehicle.ForceCallback);
                Newton.NewtonVehicleSetTireCallback(vehicle.Joint, vehicle.TireUpdateCallback);

                for (int i = 0; i < 4; i++)
                {
                    NewtonTire tire = vehicle.Tires[i];
                    if (tire == null)
                        throw new GoblinException("All of the four tires need to be set: " + ((TireID)i)
                            + " tire is null");

                    float[] localMatrix = MatrixHelper.ToFloats(tire.TireOffsetMatrix);
                    float[] pin = Vector3Helper.ToFloats(tire.Pin);
                    IntPtr newtonTireID = Newton.NewtonVehicleAddTire(vehicle.Joint, localMatrix, pin,
                        tire.Mass, tire.Width, tire.Radius, tire.SuspensionShock, tire.SuspensionSpring,
                        tire.SuspensionLength, IntPtr.Zero, tire.CollisionID);

                    vehicle.AddToTireTable(newtonTireID, i);
                }
            }

            if (!physObj.Collidable)
                Newton.NewtonWorldFreezeBody(nWorld, body);
            else
                Newton.NewtonWorldUnfreezeBody(nWorld, body);
        }
示例#30
0
        public static void Transform(Mesh mesh, Matrix m, int start, int count)
        {
            if (start < 0) start = mesh.verticies.Length - start;
            for (int i = start; i < start + count; ++i)
                mesh.verticies[i].Position = Vector3.Transform(mesh.verticies[i].Position, m);

            Vector3 scale;
            Vector3 trans;
            Quaternion rot;
            m.Decompose(out scale, out rot, out trans);
            for (int i = start; i < start + count; ++i)
                mesh.verticies[i].Normal = Vector3.Transform(mesh.verticies[i].Normal, rot);
        }
示例#31
0
 /// <summary>
 /// Update the transform by extracting the scale - rotation - position from a matrix
 /// </summary>
 /// <param name="matrix">Matrix used to update the transform</param>
 public void UpdateFromMatrix(ref Matrix matrix)
 {
     matrix.Decompose(out LocalTransformScale, out LocalTransformRotation, out LocalTransformPosition);
     RequireUpdate();
 }