/// <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 } } }