public InertiaTensor (
Vector3[] vertexStartPosition,
int[][] triangleVertexIndex,
double mass)
{
this.vertexStartPosition = vertexStartPosition;
this.triangleVertexIndex = triangleVertexIndex;
objMass = mass;
massCenter = new Vector3 ();
inertiaTensor = new Matrix3x3 ();
computeInertiaTensor ();
}
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;
}
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);
}
}
/// <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;
}
/// <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);
}
/// <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);
}
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);
}
public void SetRotationMatrix(Matrix3x3 inputRotationMatrix)
{
RotationMatrix = inputRotationMatrix;
}
public void SetInertiaTensor(Matrix3x3 inertiaTensor)
{
InertiaTensor = inertiaTensor;
}
public void SetBaseInertiaTensor(Matrix3x3 inputIntertiaTensor)
{
BaseInertiaTensor = Matrix3x3.Invert(inputIntertiaTensor);
}
