コード例 #1
0
		public InertiaTensor (
			Vector3[] vertexStartPosition,
			int[][] triangleVertexIndex,
			double mass)
		{
			this.vertexStartPosition = vertexStartPosition;
			this.triangleVertexIndex = triangleVertexIndex;
			objMass = mass;

			massCenter = new Vector3 ();
			inertiaTensor = new Matrix3x3 (); 
			computeInertiaTensor ();
		}
コード例 #2
0
		private void computeInertiaTensor()
		{
			
			for (int i = 0; i < triangleVertexIndex.Length; i++) 
			{
				//Vertice 1 triangolo

				double x0 = vertexStartPosition [triangleVertexIndex [i] [0]].x;
				double y0 = vertexStartPosition [triangleVertexIndex [i] [0]].y;
				double z0 = vertexStartPosition [triangleVertexIndex [i] [0]].z;

				//Vertice 2 triangolo

				double x1 = vertexStartPosition [triangleVertexIndex [i] [1]].x;
				double y1 = vertexStartPosition [triangleVertexIndex [i] [1]].y;
				double z1 = vertexStartPosition [triangleVertexIndex [i] [1]].z;

				//Vertice 3 triangolo

				double x2 = vertexStartPosition [triangleVertexIndex [i] [2]].x;
				double y2 = vertexStartPosition [triangleVertexIndex [i] [2]].y;
				double z2 = vertexStartPosition [triangleVertexIndex [i] [2]].z;

				//Bordi e prodotto vettoriale 

				double a1 = x1 - x0;
				double b1 = y1 - y0;
				double c1 = z1 - z0;
				double a2 = x2 - x0;
				double b2 = y2 - y0;
				double c2 = z2 - z0;
				double d0 = b1 * c2 - b2 * c1;
				double d1 = a2 * c1 - a1 * c2;
				double d2 = a1 * b2 - a2 * b1;

				//Calcolo i termini dell'integrale

				double f1x = 0.0, f2x = 0.0, f3x = 0.0, g0x = 0.0, g1x = 0.0, g2x = 0.0;
				double f1y = 0.0, f2y = 0.0, f3y = 0.0, g0y = 0.0, g1y = 0.0, g2y = 0.0;
				double f1z = 0.0, f2z = 0.0, f3z = 0.0, g0z = 0.0, g1z = 0.0, g2z = 0.0;
				subExpression (ref x0, ref x1, ref x2, ref f1x, ref f2x, ref f3x, ref g0x, ref g1x, ref g2x);
				subExpression (ref y0, ref y1, ref y2, ref f1y, ref f2y, ref f3y, ref g0y, ref g1y, ref g2y);
				subExpression (ref z0, ref z1, ref z2, ref f1z, ref f2z, ref f3z, ref g0z, ref g1z, ref g2z);

				//Aggiorno l'integrale

				intg [0] += d0 * f1x;
				intg [1] += d0*f2x; intg[2] += d1*f2y; intg [3] += d2 * f2z;
				intg [4] += d0*f3x; intg[5] += d1*f3y; intg [6] += d2 * f3z;
				intg [7] += d0 * (y0 * g0x + y1 * g1x + y2 * g2x);
				intg [8] += d1 * (z0 * g0y + z1 * g1y + z2 * g2y);
				intg [9] += d2 * (x0 * g0z + x1 * g1z + x2 * g2z);
			}
			for (int i = 0; i < 10; i++) 
			{
				intg [i] *= mult [i];
			}
			double mass = intg [0];

			//centro di massa

			double massCenterX = intg [1] / mass;
			double massCenterY = intg [2] / mass;
			double massCenterZ = intg [3] / mass;

			massCenter = new Vector3 (massCenterX, massCenterY, massCenterZ);
			//matrice tensore d'inerzia sul centro di massa

			double r1x = intg [5] + intg [6] - mass * (massCenter.y * massCenter.y + massCenter.z * massCenter.z);
			double r2y = intg [4] + intg [6] - mass * (massCenter.z * massCenter.z + massCenter.x * massCenter.x);
			double r3z = intg [4] + intg [5] - mass * (massCenter.x * massCenter.x + massCenter.y * massCenter.y);
			double r1y = -(intg [7] - mass * massCenter.x * massCenter.y);
			double r2z = -(intg [8] - mass * massCenter.y * massCenter.z);
			double r1z = -(intg [9] - mass * massCenter.z * massCenter.x);
			double r2x = -(intg [7] - mass * massCenter.x * massCenter.y);
			double r3y = -(intg [8] - mass * massCenter.y * massCenter.z);
			double r3x = -(intg [9] - mass * massCenter.z * massCenter.x);
		
			inertiaTensor = new Matrix3x3 (
				r1x, r1y, r1z,
				r2x, r2y, r2z,
				r3x, r3y, r3z);
			
			//L'oggetto ha massa totale 1, l'adatto alla massa richiesta
			double bufferMass = objMass / mass;

			inertiaTensor = inertiaTensor * bufferMass;
			
		}
コード例 #3
0
		public static Quaternion GetQuaternion(Matrix3x3 a)
		{
			double tr = a.r1c1 + a.r2c2 + a.r3c3;
			double ra = 0.0, rb = 0.0, rc = 0.0, rd = 0.0;
			double s = 0.0;

			if (tr >= 0.0) 
			{
				s = Math.Sqrt(tr + 1.0);
				ra = 0.5 * s;
				s = 0.5 / s;
				rb = (a.r3c2 - a.r2c3) * s;
				rc = (a.r1c3 - a.r3c1) * s;
				rd = (a.r2c1 - a.r1c2) * s;

				return new Quaternion (ra, rb, rc, rd);
			} 
				
			if (a.r2c2 > a.r1c1) 
			{
				if (a.r3c3 > a.r2c2) 
				{
					s = Math.Sqrt ((a.r3c3 - (a.r1c1 + a.r2c2)) + 0.5);
					rd = 0.5 * s;
					s = 0.5 / s;
					rb = (a.r3c1 + a.r1c3) * s;
					rc = (a.r2c3 + a.r3c2) * s;
					ra = (a.r2c1 - a.r1c2) * s;

					return new Quaternion (ra, rb, rc, rd);
				}

				s = Math.Sqrt ((a.r2c2 - (a.r3c3 + a.r1c1)) + 1.0);
				rc = 0.5 * s;
				s = 0.5 / s;
				rd = (a.r2c3 + a.r3c2) * s;
				rb = (a.r1c2 + a.r2c1) * s;
				ra = (a.r1c3 - a.r3c1) * s;

				return new Quaternion (ra, rb, rc, rd);
			} 
				
			if (a.r3c3 > a.r1c1) 
			{
				s = Math.Sqrt ((a.r3c3 - (a.r1c1 + a.r2c2)) + 0.5);
				rd = 0.5 * s;
				s = 0.5 / s;
				rb = (a.r3c1 + a.r1c3) * s;
				rc = (a.r2c3 + a.r3c2) * s;
				ra = (a.r2c1 - a.r1c2) * s;

				return new Quaternion (ra, rb, rc, rd);
			}
			else 
			{
				s = Math.Sqrt ((a.r1c1 - (a.r2c2 + a.r3c3)) + 1.0);
				rb = 0.5 * s;
				s = 0.5 / s;
				rc = (a.r1c2 + a.r2c1) * s;
				rd = (a.r3c1 + a.r1c3) * s;
				ra = (a.r3c2 - a.r2c3) * s;

				return new Quaternion (ra, rb, rc, rd);
			}
		}
コード例 #4
0
		/// <summary>
		/// Invert the specified matrix.
		/// </summary>
		/// <param name="a">The alpha component.</param>
		public static Matrix3x3 Invert(Matrix3x3 a)
		{
			double den = 
				-(a.r1c3 * a.r2c2 * a.r3c1)
				+(a.r1c2 * a.r2c3 * a.r3c1)
				+(a.r1c3 * a.r2c1 * a.r3c2)
				-(a.r1c1 * a.r2c3 * a.r3c2)
				-(a.r1c2 * a.r2c1 * a.r3c3)
				+(a.r1c1 * a.r2c2 * a.r3c3);
			
			if (Math.Abs(den) > 1E-100) 
			{
				den = 1.0 / den;

				double r1c1 = (-(a.r2c3 * a.r3c2) + (a.r2c2 * a.r3c3)) * den;
				double r1c2 = ((a.r1c3 * a.r3c2) - (a.r1c2 * a.r3c3)) * den;
				double r1c3 = (-(a.r1c3 * a.r2c2) + (a.r1c2 * a.r2c3)) * den;

				double r2c1 = ((a.r2c3 * a.r3c1) - (a.r2c1 * a.r3c3)) * den;
				double r2c2 = (-(a.r1c3 * a.r3c1) + (a.r1c1 * a.r3c3)) * den;
				double r2c3 = ((a.r1c3 * a.r2c1) - (a.r1c1 * a.r2c3)) * den;

				double r3c1 = (-(a.r2c2 * a.r3c1) + (a.r2c1 * a.r3c2)) * den;
				double r3c2 = ((a.r1c2 * a.r3c1) - (a.r1c1 * a.r3c2)) * den;
				double r3c3 = (-(a.r1c2 * a.r2c1) + (a.r1c1 * a.r2c2)) * den;

				return new Matrix3x3 (
					r1c1, r1c2, r1c3,
					r2c1, r2c2, r2c3,
					r3c1, r3c2, r3c3);
			} 

			return a;
		}
コード例 #5
0
		/// <summary>
		/// Normalizes the matrix rows.
		/// </summary>
		/// <returns>The rows.</returns>
		/// <param name="a">The alpha component.</param>
		public static Matrix3x3 NormalizeRows(Matrix3x3 a)
		{
			var r1 = new Vector3 (a.r1c1, a.r1c2, a.r1c3);
			var r2 = new Vector3 (a.r2c1, a.r2c2, a.r2c3);
			var r3 = new Vector3 (a.r3c1, a.r3c2, a.r3c3);
			r1 = Vector3.Normalize (r1);
			r2 = Vector3.Normalize (r2);
			r3 = Vector3.Normalize (r3);

			return new Matrix3x3 (
				r1.x, r1.y, r1.z,
				r2.x, r2.y, r2.z,
				r3.x, r3.y, r3.z);
		}
コード例 #6
0
		/// <summary>
		/// Transpose the specified matrix.
		/// </summary>
		/// <param name="a">The alpha component.</param>
		public static Matrix3x3 Transpose(Matrix3x3 a)
		{

			double r1x = a.r1c1;
			double r1y = a.r2c1;
			double r1z = a.r3c1;

			double r2x = a.r1c2;
			double r2y = a.r2c2;
			double r2z = a.r3c2;

			double r3x = a.r1c3;
			double r3y = a.r2c3;
			double r3z = a.r3c3;

			return new Matrix3x3 (
				r1x, r1y, r1z,
				r2x, r2y, r2z,
				r3x, r3y, r3z);
		}
コード例 #7
0
        private void SetObjectProperties()
        {
            Vector3 startPosition = new Vector3();
            Matrix3x3 baseTensors = new Matrix3x3();

            int totalVertex = 0;

            for (int i = 0; i < ObjectGeometry.Length; i++)
            {
                Vector3[] vertexPosition = Array.ConvertAll(
                                        ObjectGeometry[i].VertexPosition,
                                        item => item.Vertex + StartCompositePositionObjects[i]);
                //TODO da rivedere
                var inertiaTensor = new InertiaTensor(
                        vertexPosition,
                        ObjectGeometry[i].Triangle,
                        PartialMass[i]);

                var normalizedInertiaTensor = inertiaTensor;

                //Traslo per normalizzare l'oggetto rispetto al suo centro di massa
                if (inertiaTensor.GetMassCenter() != new Vector3())
                {
                    for (int j = 0; j < ObjectGeometry[i].VertexPosition.Length; j++)
                    {
                        ObjectGeometry[i].SetVertexPosition(
                            ObjectGeometry[i].VertexPosition[j].Vertex + inertiaTensor.GetMassCenter(),
                            j);
                    }

                    normalizedInertiaTensor = new InertiaTensor(
                        vertexPosition,
                        ObjectGeometry[i].Triangle,
                        PartialMass[i]);
                }

                startPosition += normalizedInertiaTensor.GetMassCenter() * PartialMass[i];
                totalVertex += ObjectGeometry[i].VertexPosition.Length;

                baseTensors += inertiaTensor.GetInertiaTensor();
            }

            RotationMatrix = Quaternion.ConvertToMatrix(Quaternion.Normalize(RotationStatus));

            if (Mass > 0)
                StartPosition = startPosition / Mass;

            SetRelativePosition(totalVertex);
            
            BaseInertiaTensor = Matrix3x3.Invert(baseTensors);
            InertiaTensor = (RotationMatrix * BaseInertiaTensor) *
                            Matrix3x3.Transpose(RotationMatrix);
        }
コード例 #8
0
		public void SetRotationMatrix(Matrix3x3 inputRotationMatrix)
		{
			RotationMatrix = inputRotationMatrix;
		}
コード例 #9
0
		public void SetInertiaTensor(Matrix3x3 inertiaTensor)
		{
			InertiaTensor = inertiaTensor;
		}
コード例 #10
0
		public void SetBaseInertiaTensor(Matrix3x3 inputIntertiaTensor)
		{
			BaseInertiaTensor = Matrix3x3.Invert(inputIntertiaTensor);
		}