/// <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>
/// Multiplies the two matrices.
/// </summary>
/// <param name="a">First matrix to multiply.</param>
/// <param name="b">Second matrix to multiply.</param>
/// <returns>Product of the multiplication.</returns>
public static Matrix3x3 operator *(Matrix3x3 a, Matrix3x3 b)
{
Matrix3x3 result;
Matrix3x3.Multiply(ref a, ref b, out result);
return(result);
}
/// <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 a convex shape description for a TransformableShape.
/// </summary>
///<param name="vA">First local vertex in the triangle.</param>
///<param name="vB">Second local vertex in the triangle.</param>
///<param name="vC">Third local vertex in the triangle.</param>
///<param name="collisionMargin">Collision margin of the shape.</param>
/// <returns>Description required to define a convex shape.</returns>
public static ConvexShapeDescription ComputeDescription(Vector3 vA, Vector3 vB, Vector3 vC, float collisionMargin)
{
ConvexShapeDescription description;
// A triangle by itself technically has no volume, but shapes try to include the collision margin in the volume when feasible (e.g. BoxShape).
//Plus, it's convenient to have a nonzero volume for buoyancy.
var doubleArea = Vector3.Cross(vB - vA, vC - vA).Length();
description.EntityShapeVolume.Volume = doubleArea * collisionMargin;
//Compute the inertia tensor.
var v = new Matrix3x3(
vA.X, vA.Y, vA.Z,
vB.X, vB.Y, vB.Z,
vC.X, vC.Y, vC.Z);
var s = new Matrix3x3(
2, 1, 1,
1, 2, 1,
1, 1, 2);
Matrix3x3.MultiplyTransposed(ref v, ref s, out description.EntityShapeVolume.VolumeDistribution);
Matrix3x3.Multiply(ref description.EntityShapeVolume.VolumeDistribution, ref v, out description.EntityShapeVolume.VolumeDistribution);
var scaling = doubleArea / 24f;
Matrix3x3.Multiply(ref description.EntityShapeVolume.VolumeDistribution, -scaling, out description.EntityShapeVolume.VolumeDistribution);
//The square-of-sum term is ignored since the parameters should already be localized (and so would sum to zero).
var sums = scaling * (vA.LengthSquared() + vB.LengthSquared() + vC.LengthSquared());
description.EntityShapeVolume.VolumeDistribution.M11 += sums;
description.EntityShapeVolume.VolumeDistribution.M22 += sums;
description.EntityShapeVolume.VolumeDistribution.M33 += sums;
description.MinimumRadius = collisionMargin;
description.MaximumRadius = collisionMargin + Math.Max(vA.Length(), Math.Max(vB.Length(), vC.Length()));
description.CollisionMargin = collisionMargin;
return(description);
}
void IForceUpdateable.UpdateForForces(float dt)
{
//Apply gravity.
if (hasPersonalGravity)
{
Vector3 gravityDt;
Vector3.Multiply(ref personalGravity, dt, out gravityDt);
Vector3.Add(ref gravityDt, ref linearVelocity, out linearVelocity);
}
else
{
Vector3.Add(ref forceUpdater.gravityDt, ref linearVelocity, out linearVelocity);
}
//Boost damping at very low velocities. This is a strong stabilizer; removes a ton of energy from the system.
if (activityInformation.DeactivationManager.useStabilization && activityInformation.allowStabilization &&
(activityInformation.isSlowing || activityInformation.velocityTimeBelowLimit > activityInformation.DeactivationManager.lowVelocityTimeMinimum))
{
float energy = linearVelocity.LengthSquared() + angularVelocity.LengthSquared();
if (energy < activityInformation.DeactivationManager.velocityLowerLimitSquared)
{
float boost = 1 - (float)(Math.Sqrt(energy) / (2f * activityInformation.DeactivationManager.velocityLowerLimit));
ModifyAngularDamping(boost);
ModifyLinearDamping(boost);
}
}
//Damping
float linear = LinearDamping + linearDampingBoost;
if (linear > 0)
{
Vector3.Multiply(ref linearVelocity, (float)Math.Pow(MathHelper.Clamp(1 - linear, 0, 1), dt), out linearVelocity);
}
//When applying angular damping, the momentum or velocity is damped depending on the conservation setting.
float angular = AngularDamping + angularDampingBoost;
if (angular > 0)
{
#if CONSERVE
Vector3.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum);
#else
Vector3.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity);
#endif
}
linearDampingBoost = 0;
angularDampingBoost = 0;
//Update world inertia tensors.
Matrix3x3 multiplied;
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse);
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor);
#if CONSERVE
//Update angular velocity.
//Note that this doesn't play nice with singular inertia tensors.
//Locked tensors result in zero angular velocity.
Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity);
MathChecker.Validate(angularMomentum);
#endif
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
}
/// <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 per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
isTryingToMove = movementDirection3d.LengthSquared() > 0;
maxForceDt = MaximumForce * dt;
maxAccelerationForceDt = MaximumAccelerationForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3 lineStart = movementDirection3d;
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
Vector3 supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3();
angularJacobianB2 = new Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirection3d;
linearJacobianA2 = Vector3.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = isTryingToMove ? TargetSpeed : 0;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3 intermediate;
inverseMass = characterBody.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2x2.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(characterBody.Mass, out massMatrix);
}
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
{
var distanceToBottomOfCharacter = supportFinder.BottomDistance;
if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
{
timeSinceTransition += dt;
}
if (timeSinceTransition >= timeUntilPositionAnchor)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3 error;
Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
///<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);
}