/// <summary>
/// Helper to calculate the combined mass, centre of mass, and
/// inertia tensor about the origin and the CoM (for the local
/// primitives) primitiveProperties is an array of properties -
/// must be the same number as there are primitives
/// </summary>
/// <param name="primitiveProperties"></param>
/// <param name="mass"></param>
/// <param name="centerOfMass"></param>
/// <param name="inertiaTensor"></param>
/// <param name="inertiaTensorCoM"></param>
public void GetMassProperties(PrimitiveProperties[] primitiveProperties,
out float mass, out Vector3 centerOfMass, out Matrix4 inertiaTensor, out Matrix4 inertiaTensorCoM)
{
mass = 0.0f;
centerOfMass = Vector3.Zero;
inertiaTensor = Matrix4.Identity;
inertiaTensorCoM = Matrix4.Identity;
for (int prim = primitivesLocal.Count; prim-- != 0;)
{
float m;
Vector3 com;
Matrix4 it;
primitivesLocal[prim].GetMassProperties(primitiveProperties[prim], out m, out com, out it);
mass += m;
centerOfMass += m * com;
inertiaTensor = Matrix4Extensions.Add(inertiaTensor, it);
}
if (mass > 0.0f)
{
centerOfMass /= mass;
// Transfer of axe theorem
inertiaTensorCoM.M11 = inertiaTensor.M11 - mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
inertiaTensorCoM.M22 = inertiaTensor.M22 - mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
inertiaTensorCoM.M33 = inertiaTensor.M33 - mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);
// CHECK THIS. seems strange for me
inertiaTensorCoM.M12 = inertiaTensorCoM.M21 = inertiaTensor.M12 + mass * centerOfMass.X * centerOfMass.Y;
inertiaTensorCoM.M23 = inertiaTensorCoM.M32 = inertiaTensor.M23 + mass * centerOfMass.Y * centerOfMass.Z;
inertiaTensorCoM.M31 = inertiaTensorCoM.M13 = inertiaTensor.M31 + mass * centerOfMass.Z * centerOfMass.X;
}
}
/// <summary>
/// Helper to calculate the combined mass, centre of mass, and
/// inertia tensor about the origin and the CoM (for the local
/// primitives) primitiveProperties indicates the properties used
/// for all primitives - so the mass is the total mass
/// </summary>
/// <param name="primitiveProperties"></param>
/// <param name="mass"></param>
/// <param name="centerOfMass"></param>
/// <param name="inertiaTensor"></param>
/// <param name="inertiaTensorCoM"></param>
public void GetMassProperties(PrimitiveProperties primitiveProperties,
out float mass, out Vector3 centerOfMass, out Matrix4 inertiaTensor, out Matrix4 inertiaTensorCoM)
{
mass = 0.0f;
centerOfMass = Vector3.Zero;
inertiaTensor = new Matrix4();
float totalWeighting = 0.0f;
if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
{
for (int prim = primitivesLocal.Count; prim-- != 0;)
{
if (primitiveProperties.MassDistribution == PrimitiveProperties.MassDistributionEnum.Solid)
{
totalWeighting += primitivesLocal[prim].GetVolume();
}
else
{
totalWeighting += primitivesLocal[prim].GetSurfaceArea();
}
}
}
for (int prim = primitivesLocal.Count; prim-- != 0;)
{
float m; Vector3 com; Matrix4 it;
PrimitiveProperties primProperties = primitiveProperties;
if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
{
float weighting = 0.0f;
if (primitiveProperties.MassDistribution == PrimitiveProperties.MassDistributionEnum.Solid)
{
weighting = primitivesLocal[prim].GetVolume();
}
else
{
weighting = primitivesLocal[prim].GetSurfaceArea();
}
primProperties.MassOrDensity *= weighting / totalWeighting;
}
primitivesLocal[prim].GetMassProperties(primProperties, out m, out com, out it);
mass += m;
centerOfMass += m * com;
inertiaTensor = Matrix4Extensions.Add(inertiaTensor, it);
}
inertiaTensorCoM = Matrix4.Identity;
if (mass > 0.0f)
{
centerOfMass /= mass;
// Transfer of axe theorem
inertiaTensorCoM.M11 = inertiaTensor.M11 - mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
inertiaTensorCoM.M22 = inertiaTensor.M22 - mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
inertiaTensorCoM.M33 = inertiaTensor.M33 - mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);
// CHECK THIS. seems strange for me
inertiaTensorCoM.M12 = inertiaTensorCoM.M21 = inertiaTensor.M12 + mass * centerOfMass.X * centerOfMass.Y;
inertiaTensorCoM.M23 = inertiaTensorCoM.M32 = inertiaTensor.M23 + mass * centerOfMass.Y * centerOfMass.Z;
inertiaTensorCoM.M31 = inertiaTensorCoM.M13 = inertiaTensor.M31 + mass * centerOfMass.Z * centerOfMass.X;
}
if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
{
mass = primitiveProperties.MassOrDensity;
}
}
/// <summary>
/// CollDetectSweep
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectSweep(ref CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
// todo - proper swept test
// note - mesh is static and its triangles are in world space
TriangleMesh mesh = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as TriangleMesh;
Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
Vector3 oldCentre;
oldBox.GetCentre(out oldCentre);
Vector3 newCentre;
newBox.GetCentre(out newCentre);
Vector3 delta;
Vector3.Subtract(ref newCentre, ref oldCentre, out delta);
float boxMinLen = 0.5f * System.Math.Min(newBox.SideLengths.X, System.Math.Min(newBox.SideLengths.Y, newBox.SideLengths.Z));
int nPositions = 1 + (int)(delta.Length / boxMinLen);
// limit the max positions...
if (nPositions > 50)
{
System.Diagnostics.Debug.WriteLine("Warning - clamping max positions in swept box test");
nPositions = 50;
}
if (nPositions == 1)
{
CollDetectBoxStaticMeshOverlap(oldBox, newBox, mesh, ref info, collTolerance, collisionFunctor);
}
else
{
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddBox(oldBox, ref bb);
BoundingBoxHelper.AddBox(newBox, ref bb);
unsafe
{
#if USE_STACKALLOC
int *potentialTriangles = stackalloc int[MaxLocalStackTris];
{
#else
int[] potTriArray = IntStackAlloc();
fixed(int *potentialTriangles = potTriArray)
{
#endif
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
if (numTriangles > 0)
{
for (int i = 0; i <= nPositions; ++i)
{
float frac = ((float)i) / nPositions;
Vector3 centre;
Vector3.Multiply(ref delta, frac, out centre);
Vector3.Add(ref centre, ref oldCentre, out centre);
Matrix4 orient = Matrix4Extensions.Add(Matrix4Extensions.Multiply(oldBox.Orientation, 1.0f - frac), Matrix4Extensions.Multiply(newBox.Orientation, frac));
Box box = new Box(centre - 0.5f * Vector3Extensions.TransformNormal(newBox.SideLengths, orient), orient, newBox.SideLengths);
// ideally we'd break if we get one collision... but that stops us getting multiple collisions
// when we enter a corner (two walls meeting) - can let us pass through
CollDetectBoxStaticMeshOverlap(oldBox, box, mesh, ref info, collTolerance, collisionFunctor);
}
}
#if USE_STACKALLOC
}
#else
}
FreeStackAlloc(potTriArray);
#endif
}
}
}
