/// <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++)
{
RigidTransform r = entries[i].LocalTransform;
center.AddScaled( ref r.Position, entries[i].Weight, out center );
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.Mult( totalWeightInverse, out center );
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();
}
CharacterContactPositionState TryUpStepPosition(ref Vector3 sideNormal, ref Vector3 position, ref Vector3 down,
ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts,
out float hintOffset)
{
hintOffset = 0;
PrepareQueryObject(ref position);
QueryManager.QueryContacts(currentQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
if (headContacts.Count > 0)
{
//The head is obstructed. This will define a maximum bound.
//Find the deepest contact on the head and use it to provide a hint.
float dot;
Vector3.Dot(ref down, ref headContacts.Elements[0].Contact.Normal, out dot);
hintOffset = -dot * headContacts.Elements[0].Contact.PenetrationDepth;
for (int i = 1; i < headContacts.Count; i++)
{
Vector3.Dot(ref down, ref headContacts.Elements[i].Contact.Normal, out dot);
dot *= -headContacts.Elements[i].Contact.PenetrationDepth;
if (dot > hintOffset)
{
hintOffset = dot;
}
}
return CharacterContactPositionState.HeadObstructed;
}
bool obstructed = IsUpStepObstructedBySideContacts(ref sideNormal, ref sideContacts);
if (!obstructed && supportContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
if (supportState == CharacterContactPositionState.Accepted)
{
if (tractionContacts.Count > 0)
{
//We're done! The guess found a good spot to stand on.
//Unlike down stepping, upstepping DOES need good contacts in the final state.
//Push it up if necessary, but don't push it too far.
//Putting it into the middle of the allowed penetration makes it very likely that it will properly generate contacts.
//Choosing something smaller than allowed penetration ensures that the search makes meaningful progress forward when the sizes get really tiny;
//we wouldn't want it edging every closer to AllowedPenetration and then exit because too many queries were made.
hintOffset = Math.Min(0, Vector3.Dot(ref supportContact.Contact.Normal, ref down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return CharacterContactPositionState.Accepted;
}
else
{
//No traction... Before we give up and reject the step altogether, let's try one last thing. It's possible that the character is trying to step up onto the side of a ramp or something.
//In this scenario, the top-down ray cast detects a perfectly walkable slope. However, the contact queries will find a contact with a normal necessarily
//steeper than the one found by the ray cast because it is an edge contact. Not being able to step up in this scenario doesn't feel natural to the player
//even if it is technically consistent.
//So, let's try to ray cast down to the a point just barely beyond the contact (to ensure we don't land right on the edge, which would invite numerical issues).
//Note that this is NOT equivalent to the ray cast we performed earlier to test for an initial step height and surface normal.
//This one is based on the QUERY state and the QUERY's contact position.
//Find the down test ray's position.
Ray downRay;
supportContact.Contact.Position.AddScaled( ref sideNormal, .001f, out downRay.Position );
Vector3 tmp;
downRay.Position.Sub( ref position, out tmp );
float verticalOffset = Vector3.Dot( ref tmp, ref down);
verticalOffset = characterBody.Height * .5f + verticalOffset;
downRay.Position.AddScaled( ref down, -verticalOffset, out downRay.Position );
downRay.Direction = down;
//First, we must ensure that the ray cast test origin is not obstructed. Starting very close to the very top of the character is safe because the process has already validated
//this location as accepted, just without traction.
Ray obstructionTestRay;
position.AddScaled( ref down, -(characterBody.Height * .5f), out obstructionTestRay.Position );
downRay.Position.Sub( ref obstructionTestRay.Position, out obstructionTestRay.Direction );
if (!QueryManager.RayCastHitAnything(ref obstructionTestRay, 1))
{
//Okay! it's safe to cast down, then.
RayHit hit;
if (QueryManager.RayCast(downRay, characterBody.Height, out hit))
{
//Got a hit!
if (characterBody.Height - maximumStepHeight < hit.T)
{
//It's in range!
float dot;
hit.Normal.Normalize();
Vector3.Dot(ref hit.Normal, ref down, out dot);
if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
{
//Slope is shallow enough to stand on!
hintOffset = Math.Min(0, Vector3.Dot(ref supportContact.Contact.Normal, ref down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
//ONE MORE thing to check. The new position of the center ray must be able to touch the ground!
downRay.Position = position;
if (QueryManager.RayCast(downRay, characterBody.Height * .5f + maximumStepHeight, out hit))
{
//It hit.. almost there!
hit.Normal.Normalize();
Vector3.Dot(ref hit.Normal, ref down, out dot);
if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
{
//It has traction! We can step!
return CharacterContactPositionState.Accepted;
}
}
}
}
}
}
//If it didn't have traction, and this was the most valid location we could find, then there is no support.
return CharacterContactPositionState.Rejected;
}
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, Vector3.Dot( ref supportContact.Contact.Normal, ref down ) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return CharacterContactPositionState.TooDeep;
}
else //if (supportState == SupportState.Separated)
{
//It's not obstructed, but the support isn't quite right.
//It's got a negative penetration depth.
//We can use that as a hint.
hintOffset = -.001f - Vector3.Dot(ref supportContact.Contact.Normal, ref down ) * supportContact.Contact.PenetrationDepth;
return CharacterContactPositionState.NoHit;
}
}
else if (obstructed)
{
return CharacterContactPositionState.Obstructed;
}
else
{
return CharacterContactPositionState.NoHit;
}
}
/// <summary>
/// Updates the movement basis of the horizontal motion constraint.
/// Should be updated automatically by the character on each time step; other code should not need to call this.
/// </summary>
/// <param name="forward">Forward facing direction of the character.</param>
public void UpdateMovementBasis(ref Vector3 forward)
{
Vector3 down;// = characterBody.orientationMatrix.Down;
characterBody.orientationMatrix.M2.Invert(out down);
forward.AddScaled(ref down, -Vector3.Dot(ref down, ref forward), out horizontalForwardDirection);
float forwardLengthSquared = horizontalForwardDirection.LengthSquared();
if (forwardLengthSquared < Toolbox.Epsilon)
{
//Use an arbitrary direction to complete the basis.
horizontalForwardDirection = characterBody.orientationMatrix.Forward;
strafeDirection = characterBody.orientationMatrix.Right;
}
else
{
Vector3.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection);
Vector3.Cross(ref down, ref horizontalForwardDirection, out strafeDirection);
//Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors.
}
Vector3.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d);
Vector3 strafeComponent;
Vector3.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent);
Vector3.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d);
}