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