/// <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>
/// Computes the volume distribution and center of the shape.
/// </summary>
/// <param name="entries">Mass-weighted entries of the compound.</param>
/// <param name="volume">Summed volume of the constituent shapes. Intersecting volumes get double counted.</param>
/// <param name="volumeDistribution">Volume distribution of the shape.</param>
/// <param name="center">Center of the compound.</param>
public static void ComputeVolumeDistribution(IList <CompoundShapeEntry> entries, out float volume, out Matrix3x3 volumeDistribution, out Vector3 center)
{
center = new Vector3();
float totalWeight = 0;
volume = 0;
for (int i = 0; i < entries.Count; i++)
{
center += entries[i].LocalTransform.Position * entries[i].Weight;
volume += entries[i].Shape.Volume;
totalWeight += entries[i].Weight;
}
if (totalWeight <= 0)
{
throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive.");
}
float totalWeightInverse = 1 / totalWeight;
totalWeightInverse.Validate();
center *= totalWeightInverse;
volumeDistribution = new Matrix3x3();
for (int i = 0; i < entries.Count; i++)
{
RigidTransform transform = entries[i].LocalTransform;
Matrix3x3 contribution;
TransformContribution(ref transform, ref center, ref entries[i].Shape.volumeDistribution, entries[i].Weight, out contribution);
Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
}
Matrix3x3.Multiply(ref volumeDistribution, totalWeightInverse, out volumeDistribution);
volumeDistribution.Validate();
}
/// <summary>
/// Computes the volume distribution of the triangle.
/// The volume distribution can be used to compute inertia tensors when
/// paired with mass and other tuning factors.
/// </summary>
///<param name="vA">First vertex in the triangle.</param>
///<param name="vB">Second vertex in the triangle.</param>
///<param name="vC">Third vertex in the triangle.</param>
/// <returns>Volume distribution of the shape.</returns>
public static Matrix3x3 ComputeVolumeDistribution(Vector3 vA, Vector3 vB, Vector3 vC)
{
Vector3 center = (vA + vB + vC) * (1 / 3f);
//Calculate distribution of mass.
const float massPerPoint = .333333333f;
//Subtract the position from the distribution, moving into a 'body space' relative to itself.
// [ (j * j + z * z) (-j * j) (-j * z) ]
//I = I + [ (-j * j) (j * j + z * z) (-j * z) ]
// [ (-j * z) (-j * z) (j * j + j * j) ]
float i = vA.X - center.X;
float j = vA.Y - center.Y;
float k = vA.Z - center.Z;
//localInertiaTensor += new Matrix(j * j + k * k, -j * j, -j * k, 0, -j * j, j * j + k * k, -j * k, 0, -j * k, -j * k, j * j + j * j, 0, 0, 0, 0, 0); //No mass per point.
var volumeDistribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k),
massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k),
massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j));
i = vB.X - center.X;
j = vB.Y - center.Y;
k = vB.Z - center.Z;
var pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k),
massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k),
massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j));
Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution);
i = vC.X - center.X;
j = vC.Y - center.Y;
k = vC.Z - center.Z;
pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k),
massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k),
massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j));
Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution);
return(volumeDistribution);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1);
Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2);
Vector3 error;
Vector3.Cross(ref worldAxisA, ref worldAxisB, out error);
Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X);
Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * this.error.X;
biasVelocity.Y = errorReduction * this.error.Y;
//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;
}
Vector3 axis1I, axis2I;
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3 inertiaTensorSum;
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum);
Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11);
Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
Matrix2x2.Negate(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);
}