/// <summary>
/// Updates the cached inverse mass and inertia.
/// </summary>
private void UpdateInverseMass()
{
// TODO: Maybe set MassInverse to dirty and do lazy evaluation?
MassInverse = (MotionType == MotionType.Dynamic) ? MassFrame.MassInverse : 0;
Vector3F inertiaInverse = (MotionType == MotionType.Dynamic) ? MassFrame.InertiaInverse : Vector3F.Zero;
if (LockRotationX)
{
inertiaInverse.X = 0;
}
if (LockRotationY)
{
inertiaInverse.Y = 0;
}
if (LockRotationZ)
{
inertiaInverse.Z = 0;
}
// TODO: make faster multiplication. Do not convert inertia vector to diagonal matrix with a lot of 0s.
Matrix33F orientationCOM = _poseCenterOfMass.Orientation;
InertiaInverseWorld = orientationCOM * Matrix33F.CreateScale(inertiaInverse) * orientationCOM.Transposed;
}
private static void GetMass(IGeometricObject geometricObject, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit,
out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
// Computes mass in parent/world space of the geometric object!
// centerOfMass is in world space and inertia is around the CM in world space!
Pose pose = geometricObject.Pose;
if ((scale.X != scale.Y || scale.Y != scale.Z) && pose.HasRotation)
{
throw new NotSupportedException("NON-UNIFORM scaling of a TransformedShape or a CompositeShape with ROTATED children is not supported.");
}
if (scale.X < 0 || scale.Y < 0 || scale.Z < 0)
{
throw new NotSupportedException("Negative scaling is not supported..");
}
var shape = geometricObject.Shape;
var totalScale = scale * geometricObject.Scale;
// Inertia around center of mass in local space.
Matrix33F inertiaCMLocal;
Vector3F centerOfMassLocal;
var body = geometricObject as RigidBody;
if (body != null)
{
// The geometric object is a rigid body and we use the properties of this body.
if (!Vector3F.AreNumericallyEqual(scale, Vector3F.One))
{
throw new NotSupportedException("Scaling is not supported when a child geometric object is a RigidBody.");
}
var massFrame = body.MassFrame;
mass = massFrame.Mass;
centerOfMassLocal = massFrame.Pose.Position;
inertiaCMLocal = massFrame.Pose.Orientation * Matrix33F.CreateScale(massFrame.Inertia) * massFrame.Pose.Orientation.Transposed;
}
else
{
// Compute new mass properties for the shape.
GetMass(shape, totalScale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit,
out mass, out centerOfMassLocal, out inertiaCMLocal);
}
// Get inertia around center of mass in world space:
// The world inertia is equal to: move to local space using the inverse orientation, then
// apply local inertia then move to world space with the orientation matrix.
inertia = pose.Orientation * inertiaCMLocal * pose.Orientation.Transposed;
// Convert center of mass offset in world space. - Do not forget scale!
centerOfMass = pose.ToWorldPosition(centerOfMassLocal) * scale;
}
public void InfiniteShape()
{
var s = new InfiniteShape();
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 0.7f, true, 0.001f, 4, out m0, out com0, out i0);
Assert.AreEqual(float.PositiveInfinity, m0);
Assert.AreEqual(Vector3F.Zero, com0);
Assert.AreEqual(Matrix33F.CreateScale(float.PositiveInfinity), i0);
}
public ConvexHullSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
SetCamera(new Vector3F(0, 1, 10), 0, 0);
// Generate random points.
var points = new List <Vector3F>();
for (int i = 0; i < 100; i++)
{
points.Add(RandomHelper.Random.NextVector3F(-1, 1));
}
// Apply random transformation to points to make this sample more interesting.
Matrix44F transform = new Matrix44F(
Matrix33F.CreateRotation(RandomHelper.Random.NextQuaternionF()) * Matrix33F.CreateScale(RandomHelper.Random.NextVector3F(0.1f, 2f)),
RandomHelper.Random.NextVector3F(-1, 1));
for (int i = 0; i < points.Count; i++)
{
points[i] = transform.TransformPosition(points[i]);
}
// Compute convex hull. The result is the mesh of the hull represented as a
// Doubly-Connected Edge List (DCEL).
DcelMesh convexHull = GeometryHelper.CreateConvexHull(points);
// We don't need the DCEL representation. Let's store the hull as a simpler triangle mesh.
TriangleMesh convexHullMesh = convexHull.ToTriangleMesh();
// Compute a tight-fitting oriented bounding box.
Vector3F boundingBoxExtent; // The bounding box dimensions (widths in X, Y and Z).
Pose boundingBoxPose; // The pose (world space position and orientation) of the bounding box.
GeometryHelper.ComputeBoundingBox(points, out boundingBoxExtent, out boundingBoxPose);
// (Note: The GeometryHelper also contains methods to compute a bounding sphere.)
var debugRenderer = GraphicsScreen.DebugRenderer;
foreach (var point in points)
{
debugRenderer.DrawPoint(point, Color.White, true);
}
debugRenderer.DrawShape(new TriangleMeshShape(convexHullMesh), Pose.Identity, Vector3F.One, Color.Violet, false, false);
debugRenderer.DrawBox(boundingBoxExtent.X, boundingBoxExtent.Y, boundingBoxExtent.Z, boundingBoxPose, Color.Red, true, false);
}
/// <summary>
/// Computes the K matrix needed by sequential impulse-based methods.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="positionWorld">The constraint anchor position in world space.</param>
/// <returns>The K matrix.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="body"/> is <see langword="null"/>.
/// </exception>
public static Matrix33F ComputeKMatrix(this RigidBody body, Vector3F positionWorld)
{
if (body == null)
{
throw new ArgumentNullException("body");
}
if (body.MotionType != MotionType.Dynamic)
{
return(Matrix33F.Zero);
}
Vector3F radiusVector = positionWorld - body.PoseCenterOfMass.Position;
Matrix33F skewR = radiusVector.ToCrossProductMatrix();
Matrix33F massMatrixInverse = Matrix33F.CreateScale(body.MassInverse);
Matrix33F kMatrix = massMatrixInverse - skewR * body.InertiaInverseWorld * skewR;
return(kMatrix);
}
public void CreateScale()
{
Matrix33F i = Matrix33F.CreateScale(1.0f);
Assert.AreEqual(Matrix33F.Identity, i);
Vector3F v = Vector3F.One;
Matrix33F m = Matrix33F.CreateScale(2.0f);
Assert.AreEqual(2 * v, m * v);
m = Matrix33F.CreateScale(-1.0f, 1.5f, 2.0f);
Assert.AreEqual(new Vector3F(-1.0f, 1.5f, 2.0f), m * v);
Vector3F scale = new Vector3F(-2.0f, -3.0f, -4.0f);
m = Matrix33F.CreateScale(scale);
v = new Vector3F(1.0f, 2.0f, 3.0f);
Assert.AreEqual(v * scale, m * v);
}
public void DiagonalizeTest()
{
for (int i = 0; i < 1000; i++)
{
var inertia = RandomHelper.Random.NextMatrix33F(0, 100);
// Make symmetric
inertia.M10 = inertia.M01;
inertia.M12 = inertia.M21;
inertia.M20 = inertia.M02;
Assert.IsTrue(inertia.IsSymmetric);
Vector3F inertiaDiagonale;
Matrix33F rotation;
MassHelper.DiagonalizeInertia(inertia, out inertiaDiagonale, out rotation);
Assert.IsTrue(rotation.IsRotation);
var inertia2 = rotation * Matrix33F.CreateScale(inertiaDiagonale) * rotation.Transposed;
Assert.IsTrue(Matrix33F.AreNumericallyEqual(inertia, inertia2, 0.001f)); // Epsilon = 10^-4 is already too small :-(
}
}
public void ComputeKMatrix()
{
var b = new RigidBody(new EmptyShape());
b.MassFrame = new MassFrame()
{
Mass = 3,
Inertia = new Vector3F(0.4f, 0.5f, 0.6f),
};
var r = new Vector3F(1, 2, 3);
var k = ConstraintHelper.ComputeKMatrix(b, r);
var desiredK = 1 / b.MassFrame.Mass * Matrix33F.Identity - r.ToCrossProductMatrix() * Matrix33F.CreateScale(b.MassFrame.InertiaInverse) * r.ToCrossProductMatrix();
Assert.AreEqual(desiredK, k);
}
internal static void GetMass(Shape shape, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit,
out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
if (shape == null)
{
throw new ArgumentNullException("shape");
}
if (densityOrMass <= 0)
{
throw new ArgumentOutOfRangeException("densityOrMass", "The density or mass must be greater than 0.");
}
if (relativeDistanceThreshold < 0)
{
throw new ArgumentOutOfRangeException("relativeDistanceThreshold", "The relative distance threshold must not be negative.");
}
mass = 0;
centerOfMass = Vector3F.Zero;
inertia = Matrix33F.Zero;
// Note: We support all shape types of DigitalRune Geometry.
// To support user-defined shapes we could add an interface IMassSource which can be
// implemented by shapes. In the else-case below we can check whether the shape implements
// the interface.
if (shape is EmptyShape)
{
return;
}
else if (shape is InfiniteShape)
{
mass = float.PositiveInfinity;
inertia = Matrix33F.CreateScale(float.PositiveInfinity);
}
else if (shape is BoxShape)
{
GetMass((BoxShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is CapsuleShape)
{
GetMass((CapsuleShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is ConeShape)
{
GetMass((ConeShape)shape, scale, densityOrMass, isDensity, out mass, out centerOfMass, out inertia);
}
else if (shape is CylinderShape)
{
GetMass((CylinderShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is ScaledConvexShape)
{
var scaledConvex = (ScaledConvexShape)shape;
GetMass(scaledConvex.Shape, scale * scaledConvex.Scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit, out mass, out centerOfMass, out inertia);
}
else if (shape is SphereShape)
{
GetMass((SphereShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is TransformedShape)
{
var transformed = (TransformedShape)shape;
// Call GetMass for the contained GeometricObject.
GetMass(transformed.Child, scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit, out mass, out centerOfMass, out inertia);
}
else if (shape is HeightField)
{
// Height fields should always be static. Therefore, they we can treat them as having
// infinite or zero mass.
return;
}
else if (shape is CompositeShape)
{
var composite = (CompositeShape)shape;
float density = (isDensity) ? densityOrMass : 1;
foreach (var child in composite.Children)
{
// Call GetMass for the child geometric object.
float childMass;
Vector3F childCenterOfMass;
Matrix33F childInertia;
GetMass(child, scale, density, true, relativeDistanceThreshold, iterationLimit, out childMass, out childCenterOfMass, out childInertia);
// Add child mass to total mass.
mass = mass + childMass;
// Add child inertia to total inertia and consider the translation.
inertia += GetTranslatedMassInertia(childMass, childInertia, childCenterOfMass);
// Add weighted centerOfMass.
centerOfMass = centerOfMass + childCenterOfMass * childMass;
}
// centerOfMass must be divided by total mass because child center of mass were weighted
// with the child masses.
centerOfMass /= mass;
// Make inertia relative to center of mass.
inertia = GetUntranslatedMassInertia(mass, inertia, centerOfMass);
if (!isDensity)
{
// Yet, we have not computed the correct total mass. We have to adjust the total mass to
// be equal to the given target mass.
AdjustMass(densityOrMass, ref mass, ref inertia);
}
}
else if (iterationLimit <= 0)
{
// We do not have a special formula for this kind of shape and iteration limit is 0 or less.
// --> Use mass properties of AABB.
var aabb = shape.GetAabb(scale, Pose.Identity);
var extent = aabb.Extent;
centerOfMass = aabb.Center;
GetMass(extent, densityOrMass, isDensity, out mass, out inertia);
}
else
{
// We do not have a special formula for this kind of shape.
// --> General polyhedron mass from triangle mesh.
var mesh = shape.GetMesh(relativeDistanceThreshold, iterationLimit);
mesh.Transform(Matrix44F.CreateScale(scale));
GetMass(mesh, out mass, out centerOfMass, out inertia);
// Mass was computed for density = 1. --> Scale mass.
if (isDensity)
{
var volume = mesh.GetVolume();
var targetMass = volume * densityOrMass;
AdjustMass(targetMass, ref mass, ref inertia);
}
else
{
AdjustMass(densityOrMass, ref mass, ref inertia);
}
if (Numeric.IsLessOrEqual(mass, 0))
{
// If the mass is not valid, we fall back to the AABB mass.
// This can happen for non-closed meshes that have a "negative" volume.
GetMass(shape, scale, densityOrMass, isDensity, relativeDistanceThreshold, -1, out mass, out centerOfMass, out inertia);
return;
}
}
}
