/// <summary> /// Modifies a contribution using a transform, position, and weight. /// </summary> /// <param name="transform">Transform to use to modify the contribution.</param> /// <param name="center">Center to use to modify the contribution.</param> /// <param name="baseContribution">Original unmodified contribution.</param> /// <param name="weight">Weight of the contribution.</param> /// <param name="contribution">Transformed contribution.</param> public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution) { Matrix3x3 rotation; Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation); Matrix3x3 temp; //Do angular transformed contribution first... Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp); Matrix3x3.Multiply(ref temp, ref rotation, out temp); contribution = temp; //Now add in the offset from the origin. Vector3 offset; Vector3.Subtract(ref transform.Position, ref center, out offset); Matrix3x3 innerProduct; Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct); Matrix3x3 outerProduct; Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct); Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp); Matrix3x3.Add(ref contribution, ref temp, out contribution); Matrix3x3.Multiply(ref contribution, weight, out contribution); }
/// <summary> /// Calculates necessary information for velocity solving. /// Called by preStep(float dt) /// </summary> /// <param name="dt">Time in seconds since the last update.</param> public override void Update(float dt) { Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); float errorReductionParameter; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness); //Mass Matrix Matrix3x3 k; Matrix3x3 linearComponent; Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct); Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct); if (connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent); Matrix3x3 angularComponentA, angularComponentB; Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA); Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB); Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB); Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k); Matrix3x3.Subtract(ref k, ref angularComponentB, out k); } else if (connectionA.isDynamic && !connectionB.isDynamic) { Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent); Matrix3x3 angularComponentA; Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA); Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k); } else if (!connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent); Matrix3x3 angularComponentB; Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB); Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB); Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k); } else { throw new InvalidOperationException("Cannot constrain two kinematic bodies."); } k.M11 += softness; k.M22 += softness; k.M33 += softness; Matrix3x3.Invert(ref k, out massMatrix); Vector3.Add(ref connectionB.position, ref worldOffsetB, out error); Vector3.Subtract(ref error, ref connectionA.position, out error); Vector3.Subtract(ref error, ref worldOffsetA, out error); Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity); //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } }
/// <summary> /// Computes the center, volume, and volume distribution of a shape represented by a mesh. /// </summary> /// <param name="vertices">Vertices of the mesh.</param> /// <param name="triangleIndices">Groups of 3 indices into the vertices array which represent the triangles of the mesh.</param> /// <param name="center">Computed center of the shape's volume.</param> /// <param name="volume">Volume of the shape.</param> /// <param name="volumeDistribution">Distribution of the volume as measured from the computed center.</param> public static void ComputeShapeDistribution(IList <Vector3> vertices, IList <int> triangleIndices, out Vector3 center, out float volume, out Matrix3x3 volumeDistribution) { //Explanation for the tetrahedral integration bits: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates //http://www.scipub.org/fulltext/jms2/jms2118-11.pdf //x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3 //Looking to find inertia tensor matrix of the form // [ a -b' -c' ] // [ -b' b -a' ] // [ -c' -a' c ] float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0; Vector3 summedCenter = new Vector3(); float scaledVolume = 0; for (int i = 0; i < triangleIndices.Count; i += 3) { Vector3 v2 = vertices[triangleIndices[i]]; Vector3 v3 = vertices[triangleIndices[i + 1]]; Vector3 v4 = vertices[triangleIndices[i + 2]]; //Determinant is 6 * volume. It's signed, though; the mesh isn't necessarily convex and the origin isn't necessarily in the mesh even if it is convex. float scaledTetrahedronVolume = v2.X * (v3.Z * v4.Y - v3.Y * v4.Z) - v3.X * (v2.Z * v4.Y - v2.Y * v4.Z) + v4.X * (v2.Z * v3.Y - v2.Y * v3.Z); scaledVolume += scaledTetrahedronVolume; Vector3 tetrahedronCentroid; Vector3.Add(ref v2, ref v3, out tetrahedronCentroid); Vector3.Add(ref tetrahedronCentroid, ref v4, out tetrahedronCentroid); Vector3.Multiply(ref tetrahedronCentroid, scaledTetrahedronVolume, out tetrahedronCentroid); Vector3.Add(ref tetrahedronCentroid, ref summedCenter, out summedCenter); a += scaledTetrahedronVolume * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); b += scaledTetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); c += scaledTetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y); ao += scaledTetrahedronVolume * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z); bo += scaledTetrahedronVolume * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z); co += scaledTetrahedronVolume * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y); } if (scaledVolume < Toolbox.Epsilon) { //This function works on the assumption that there is volume. //If there is no volume, then volume * density is 0, so the mass is considered to be zero. //If the mass is zero, then a zero matrix is the consistent result. //In other words, this function shouldn't be used with things with no volume. //A special case should be used instead. volumeDistribution = new Matrix3x3(); volume = 0; center = new Vector3(); } else { Vector3.Multiply(ref summedCenter, 0.25f / scaledVolume, out center); volume = scaledVolume / 6; float scaledDensity = 1 / volume; float diagonalFactor = scaledDensity / 60; float offFactor = -scaledDensity / 120; a *= diagonalFactor; b *= diagonalFactor; c *= diagonalFactor; ao *= offFactor; bo *= offFactor; co *= offFactor; //volumeDistribution = new Matrix3x3(a, bo, co, // bo, b, ao, // co, ao, c); volumeDistribution = new Matrix3x3(a, co, bo, co, b, ao, bo, ao, c); //The volume distribution, as computed, is currently offset from the origin. //There's a operation that moves a local inertia tensor to a displaced position. //The inverse of that operation can be computed and applied to the displaced inertia to center it on the origin. Matrix3x3 additionalInertia; GetPointContribution(1, ref Toolbox.ZeroVector, ref center, out additionalInertia); Matrix3x3.Subtract(ref volumeDistribution, ref additionalInertia, out volumeDistribution); //The derivation that shows the above point mass usage is valid goes something like this, with lots of details left out: //Consider the usual form of the tensor, created from the summation of a bunch of pointmasses representing the shape. //Each sum contribution relies on a particular offset, r. When the center of mass isn't aligned with (0,0,0), //r = c + b, where c is the center of mass and b is the offset of r from the center of mass. //So, each term of the matrix (like M11 = sum(mi * (ry*ry + rz*rz))) can be rephrased in terms of the center and the offset: //M11 = sum(mi * ((cy + by) * (cy + by) + (cz + bz) * (cz + bz))) //Expanding that gets you to: //M11 = sum(mi * (cycy + 2cyby + byby + czcz + 2czbz + bzbz)) //A couple of observations move things along. //1) Since it's constant over the summation, the c terms can be pulled out of a sum. //2) sum(mi * by) and sum(mi * bz) are zero, because 'by' and 'bz' are offsets from the center of mass. In other words, if you averaged all of the offsets, it would equal (0,0,0). //(uniform density assumed) //With a little more massaging, the constant c terms can be fully separated into an additive term on each matrix element. } }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //Transform point into world space. Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r); Vector3.Add(ref r, ref entity.position, out worldPoint); float updateRate = 1 / dt; if (settings.mode == MotorMode.Servomechanism) { Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error); float separationDistance = error.Length(); if (separationDistance > Toolbox.BigEpsilon) { float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness); //The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity. //The constant correction velocity could overshoot the destination, so clamp it. float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) + separationDistance * errorReduction; Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity); //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { //Wouldn't want to use a bias from an earlier frame. biasVelocity = new Vector3(); } } else { usedSoftness = settings.velocityMotor.softness * updateRate; biasVelocity = settings.velocityMotor.goalVelocity; error = Vector3.Zero; } //Compute the maximum force that can be applied this frame. ComputeMaxForces(settings.maximumForce, dt); //COMPUTE EFFECTIVE MASS MATRIX //Transforms a change in velocity to a change in momentum when multiplied. Matrix3x3 linearComponent; Matrix3x3.CreateScale(entity.inverseMass, out linearComponent); Matrix3x3 rACrossProduct; Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct); Matrix3x3 angularComponentA; Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA); Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix); effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); }
/// <summary> /// Recenters the triangle data and computes the volume distribution. /// </summary> /// <param name="data">Mesh data to analyze.</param> /// <returns>Computed center, volume, and volume distribution.</returns> private ShapeDistributionInformation ComputeVolumeDistribution(TransformableMeshData data) { //Compute the surface vertices of the shape. ShapeDistributionInformation shapeInformation; if (solidity == MobileMeshSolidity.Solid) { //The following inertia tensor calculation assumes a closed mesh. var transformedVertices = CommonResources.GetVectorList(); if (transformedVertices.Capacity < data.vertices.Length) { transformedVertices.Capacity = data.vertices.Length; } transformedVertices.Count = data.vertices.Length; for (int i = 0; i < data.vertices.Length; ++i) { data.GetVertexPosition(i, out transformedVertices.Elements[i]); } InertiaHelper.ComputeShapeDistribution(transformedVertices, data.indices, out shapeInformation.Center, out shapeInformation.Volume, out shapeInformation.VolumeDistribution); CommonResources.GiveBack(transformedVertices); if (shapeInformation.Volume > 0) { return(shapeInformation); } throw new ArgumentException("A solid mesh must have volume."); } shapeInformation.Center = new Vector3(); shapeInformation.VolumeDistribution = new Matrix3x3(); float totalWeight = 0; for (int i = 0; i < data.indices.Length; i += 3) { //Compute the center contribution. Vector3 vA, vB, vC; data.GetTriangle(i, out vA, out vB, out vC); Vector3 vAvB; Vector3 vAvC; Vector3.Subtract(ref vB, ref vA, out vAvB); Vector3.Subtract(ref vC, ref vA, out vAvC); Vector3 cross; Vector3.Cross(ref vAvB, ref vAvC, out cross); float weight = cross.Length(); totalWeight += weight; float perVertexWeight = weight * (1f / 3f); shapeInformation.Center += perVertexWeight * (vA + vB + vC); //Compute the inertia contribution of this triangle. //Approximate it using pointmasses positioned at the triangle vertices. //(There exists a direct solution, but this approximation will do plenty fine.) Matrix3x3 aContribution, bContribution, cContribution; InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vA, out aContribution); InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vB, out bContribution); InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vC, out cContribution); Matrix3x3.Add(ref aContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution); Matrix3x3.Add(ref bContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution); Matrix3x3.Add(ref cContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution); } shapeInformation.Center /= totalWeight; //The extra factor of 2 is used because the cross product length was twice the actual area. Matrix3x3.Multiply(ref shapeInformation.VolumeDistribution, 1 / (2 * totalWeight), out shapeInformation.VolumeDistribution); //Move the inertia tensor into position according to the center. Matrix3x3 additionalInertia; InertiaHelper.GetPointContribution(0.5f, ref Toolbox.ZeroVector, ref shapeInformation.Center, out additionalInertia); Matrix3x3.Subtract(ref shapeInformation.VolumeDistribution, ref additionalInertia, out shapeInformation.VolumeDistribution); shapeInformation.Volume = 0; return(shapeInformation); }