public RigidBody_c(Body_c b, Shape cg, HullMaker hull, float radius, Microsoft.Xna.Framework.Graphics.Color c)
{
maxRadius = radius;
body = b;
collideModel = cg;
renderModel = hull;
id = idcounter;
colour = c;
idcounter++;
}
void ComputeMassProperties(Body_c body, HullMaker model, float density)
{
MyVector3 diag = new MyVector3(Vector3.Zero);
MyVector3 offDiag = new MyVector3(Vector3.Zero);
Vector3 weightedCenterOfMass = Vector3.Zero;
float volume = 0;
float mass = 0;
// Iterate through the faces
for (int faceIndex = 0; faceIndex < model.surfaceTriList.Count; faceIndex++)
{
HullMaker.ClipTri face = model.surfaceTriList[faceIndex];
// Iterate through the tris in the face
for (int triIndex = 0; triIndex < 3; triIndex++)
{
MyVector3 v0 = new MyVector3(face.n1);
MyVector3 v1 = new MyVector3(face.n2);
MyVector3 v2 = new MyVector3(face.n3);
float det = Det(v0.V3(), v1.V3(), v2.V3());
// Volume
float tetVolume = det / 6.0f;
volume += tetVolume;
// Mass
float tetMass = tetVolume * density;
mass += tetMass;
// Center of Mass
Vector3 tetCenterOfMass = ((v0 + v1 + v2) / 4.0f).V3(); // Note: includes origin (0, 0, 0) as fourth vertex
weightedCenterOfMass += tetMass * tetCenterOfMass;
// Inertia Tensor
for (int i = 0; i < 3; i++)
{
int j = (i + 1) % 3;
int k = (i + 2) % 3;
diag[i] += det * (v0[i] * v1[i] + v1[i] * v2[i] + v2[i] * v0[i] + v0[i] * v0[i] + v1[i] * v1[i] + v2[i] * v2[i]) / 60.0f;
offDiag[i] += det * (
v0[j] * v1[k] + v1[j] * v2[k] + v2[j] * v0[k] +
v0[j] * v2[k] + v1[j] * v0[k] + v2[j] * v1[k] +
2 * v0[j] * v0[k] + 2 * v1[j] * v1[k] + 2 * v2[j] * v2[k]) / 120.0f;
}
}
}
Debug_c.Assert(mass > 0);
if (mass == 0.0f)
{
mass = 5.0f;
}
Vector3 centerOfMass = weightedCenterOfMass / mass;
diag *= density;
offDiag *= density;
MyMatrix I = new MyMatrix(Matrix.Identity);
I[0, 0] = diag[1] + diag[2];
I[1, 1] = diag[2] + diag[0];
I[2, 2] = diag[0] + diag[1];
I[1, 2] = I[2, 1] = -offDiag[0];
I[0, 2] = I[2, 0] = -offDiag[1];
I[0, 1] = I[1, 0] = -offDiag[2];
///
// Move inertia tensor to be relative to center of mass (rather than origin)
// Translate intertia to center of mass
float x = centerOfMass.X;
float y = centerOfMass.Y;
float z = centerOfMass.Z;
//Debug_c.Assert(Math.Abs(x)>0);
//Debug_c.Assert(Math.Abs(y)>0);
//Debug_c.Assert(Math.Abs(z)>0);
//if (x==0.0f) x = 1.0f;
//if (y==0.0f) y = 1.0f;
//if (z==0.0f) z = 1.0f;
I[0, 0] -= mass * (y * y + z * z);
I[0, 1] -= mass * (-x * y);
I[0, 2] -= mass * (-x * z);
I[1, 1] -= mass * (x * x + z * z);
I[1, 2] -= mass * (-y * z);
I[2, 2] -= mass * (x * x + y * y);
// Symmetry
I[1, 0] = I[0, 1];
I[2, 0] = I[0, 2];
I[2, 1] = I[1, 2];
float check = 0.0f;
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
check += I[r, c];
}
}
Debug_c.Assert(Math.Abs(check) > 0.0f);
if (check == 0.0f)
{
I = new MyMatrix(Matrix.Identity);
}
body.com = centerOfMass;
body.inv_m = 1.0f / mass;
body.inv_m_back = body.inv_m;
body.I = I.Get();
GeneralInverse4x4(out body.inv_I, ref I);
//body.inv_I = Matrix.Invert( I.Get() );
Debug_c.Valid(body.com);
Debug_c.Valid(body.inv_m);
Debug_c.Valid(body.inv_I);
Debug_c.Valid(body.I);
Matrix test = Matrix.Identity;
test = Matrix.Invert(I.Get());
}
RigidBody_c CreateRigidBody(Shape collideModel, Quaternion q, Vector3 x, float inv_m)
{
//RenderPolytope renderModel = rb.renderModel;
HullMaker renderModel = new HullMaker(collideModel, -1);
//if (!renderModel)
//{
// renderModel = CreateRenderModel(collideModel);
//}
Body_c body = new Body_c();
body.q = q;
body.x = x;
body.inv_I = Matrix.Identity;
body.inv_I *= inv_m / 25.0f;
//body.inv_I.M33 = 1;
body.inv_I.M44 = 1;
body.inv_m = inv_m;
if (inv_m == 0.0f)
{
body.m = 0.0f;
}
if (inv_m > 0.0f)
{
HelperRigidBody_c.ComputeMassProperties(body, renderModel, 1.0f);
}
float radiusNegX = Math.Abs(collideModel.GetSupportPoint(new Vector3(-1, 0, 0)).X);
float radiusPosX = Math.Abs(collideModel.GetSupportPoint(new Vector3(1, 0, 0)).X);
float radiusNegY = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, -1, 0)).Y);
float radiusPosY = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 1, 0)).Y);
float radiusNegZ = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 0, -1)).Z);
float radiusPosZ = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 0, 1)).Z);
Vector3 maxRadiusVector = new Vector3
(
Math.Max(radiusNegX, radiusPosX),
Math.Max(radiusNegY, radiusPosY),
Math.Max(radiusNegZ, radiusPosZ)
);
float maxRadius = maxRadiusVector.Length();
Microsoft.Xna.Framework.Graphics.Color[] randColour = { Microsoft.Xna.Framework.Graphics.Color.Red,
Microsoft.Xna.Framework.Graphics.Color.Green,
Microsoft.Xna.Framework.Graphics.Color.PaleGoldenrod,
Microsoft.Xna.Framework.Graphics.Color.OliveDrab,
Microsoft.Xna.Framework.Graphics.Color.OrangeRed,
Microsoft.Xna.Framework.Graphics.Color.Wheat,
Microsoft.Xna.Framework.Graphics.Color.YellowGreen };
randColourIndx++;
if (randColourIndx > (randColour.Count() - 1))
{
randColourIndx = 0;
}
body.inv_m_back = body.inv_m;
return(new RigidBody_c(body, collideModel, renderModel, maxRadius, randColour[randColourIndx]));
}
