/// <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 System.Numerics.Vector3 forward)
{
System.Numerics.Vector3 down = characterBody.orientationMatrix.Down;
System.Numerics.Vector3 strafeDirection;
System.Numerics.Vector3 horizontalForwardDirection = forward - down * Vector3Ex.Dot(down, forward);
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
{
Vector3Ex.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection);
Vector3Ex.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.
}
Vector3Ex.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d);
System.Numerics.Vector3 strafeComponent;
Vector3Ex.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent);
Vector3Ex.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d);
}
static void SplitReposition(Entity a, Entity b, ref ShapeDistributionInformation distributionInfoA, ref ShapeDistributionInformation distributionInfoB, float weightA, float weightB)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
System.Numerics.Vector3 weightedA, weightedB;
Vector3Ex.Multiply(ref distributionInfoA.Center, weightA, out weightedA);
Vector3Ex.Multiply(ref distributionInfoB.Center, weightB, out weightedB);
System.Numerics.Vector3 newLocalCenter;
Vector3Ex.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3Ex.Divide(ref newLocalCenter, weightA + weightB, out newLocalCenter);
System.Numerics.Vector3 localOffsetA;
System.Numerics.Vector3 localOffsetB;
Vector3Ex.Subtract(ref distributionInfoA.Center, ref newLocalCenter, out localOffsetA);
Vector3Ex.Subtract(ref distributionInfoB.Center, ref newLocalCenter, out localOffsetB);
System.Numerics.Vector3 originalPosition = a.position;
b.Orientation = a.Orientation;
System.Numerics.Vector3 offsetA = QuaternionEx.Transform(localOffsetA, a.Orientation);
System.Numerics.Vector3 offsetB = QuaternionEx.Transform(localOffsetB, a.Orientation);
a.Position = originalPosition + offsetA;
b.Position = originalPosition + offsetB;
System.Numerics.Vector3 originalLinearVelocity = a.linearVelocity;
System.Numerics.Vector3 originalAngularVelocity = a.angularVelocity;
a.AngularVelocity = originalAngularVelocity;
b.AngularVelocity = originalAngularVelocity;
a.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offsetA);
b.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offsetB);
}
/// <summary>
/// Gets the angular velocity necessary to change an entity's orientation from
/// the starting quaternion to the ending quaternion over time dt.
/// </summary>
/// <param name="start">Initial position.</param>
/// <param name="end">Final position.</param>
/// <param name="dt">Time over which the angular velocity is to be applied.</param>
/// <returns>Angular velocity to reach the goal in time.</returns>
public static System.Numerics.Vector3 GetLinearVelocity(System.Numerics.Vector3 start, System.Numerics.Vector3 end, float dt)
{
System.Numerics.Vector3 offset;
Vector3Ex.Subtract(ref end, ref start, out offset);
Vector3Ex.Divide(ref offset, dt, out offset);
return(offset);
}
/// <summary>
/// Automatically computes the measurement axes for the current local axes.
/// The current relative state of the entities will be considered 0 twist angle.
/// </summary>
public void ComputeMeasurementAxes()
{
System.Numerics.Vector3 axisA, axisB;
QuaternionEx.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
QuaternionEx.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
//Pick an axis perpendicular to axisA to use as the measurement axis.
System.Numerics.Vector3 worldMeasurementAxisA;
Vector3Ex.Cross(ref Toolbox.UpVector, ref axisA, out worldMeasurementAxisA);
float lengthSquared = worldMeasurementAxisA.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref worldMeasurementAxisA, (float)Math.Sqrt(lengthSquared), out worldMeasurementAxisA);
}
else
{
//Oops! It was parallel to the up vector. Just try again with the right vector.
Vector3Ex.Cross(ref Toolbox.RightVector, ref axisA, out worldMeasurementAxisA);
worldMeasurementAxisA.Normalize();
}
//Attach the measurement axis to entity B.
//'Push' A's axis onto B by taking into account the swing transform.
System.Numerics.Quaternion alignmentRotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref axisA, ref axisB, out alignmentRotation);
System.Numerics.Vector3 worldMeasurementAxisB;
QuaternionEx.Transform(ref worldMeasurementAxisA, ref alignmentRotation, out worldMeasurementAxisB);
//Plop them on!
MeasurementAxisA = worldMeasurementAxisA;
MeasurementAxisB = worldMeasurementAxisB;
}
static void RemoveReposition(Entity compound, ref ShapeDistributionInformation distributionInfo, float weight, float removedWeight, ref System.Numerics.Vector3 removedCenter)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
System.Numerics.Vector3 weightedA, weightedB;
Vector3Ex.Multiply(ref distributionInfo.Center, weight, out weightedA);
Vector3Ex.Multiply(ref removedCenter, removedWeight, out weightedB);
System.Numerics.Vector3 newLocalCenter;
Vector3Ex.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3Ex.Divide(ref newLocalCenter, weight + removedWeight, out newLocalCenter);
System.Numerics.Vector3 localOffset;
Vector3Ex.Subtract(ref distributionInfo.Center, ref newLocalCenter, out localOffset);
System.Numerics.Vector3 originalPosition = compound.position;
System.Numerics.Vector3 offset = QuaternionEx.Transform(localOffset, compound.orientation);
compound.Position = originalPosition + offset;
System.Numerics.Vector3 originalLinearVelocity = compound.linearVelocity;
System.Numerics.Vector3 originalAngularVelocity = compound.angularVelocity;
compound.AngularVelocity = originalAngularVelocity;
compound.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offset);
}
//public static void GetInertiaOffset(System.Numerics.Vector3 offset, float mass, out Matrix3x3 additionalInertia)
//{
// additionalInertia.M11 = mass * (offset.Y * offset.Y + offset.Z * offset.Z);
// additionalInertia.M12 = -mass * offset.X * offset.Y;
// additionalInertia.M13 = -mass * offset.X * offset.Z;
// additionalInertia.M21 = -mass * offset.Y * offset.X;
// additionalInertia.M22 = mass * (offset.X * offset.X + offset.Z * offset.Z);
// additionalInertia.M23 = -mass * offset.Y * offset.Z;
// additionalInertia.M31 = -mass * offset.Z * offset.X;
// additionalInertia.M32 = -mass * offset.Z * offset.Y;
// additionalInertia.M33 = mass * (offset.X * offset.X + offset.Y * offset.Y);
//}
/// <summary>
/// Computes a minimum radius estimate of a shape based on a convex mesh representation.
/// </summary>
/// <param name="vertices">Vertices of the convex mesh.</param>
/// <param name="triangleIndices">Groups of 3 indices into the vertices array which represent the triangles of the convex mesh.</param>
/// <param name="center">Center of the convex shape.</param>
public static float ComputeMinimumRadius(IList <System.Numerics.Vector3> vertices, IList <int> triangleIndices, ref System.Numerics.Vector3 center)
{
//Walk through all of the triangles. Treat them as a bunch of planes which bound the shape.
//The closest distance on any of those planes to the center is the radius of the largest sphere,
//centered on the... center, which can fit in the shape.
//While this shares a lot of math with the volume distribution computation (volume of a parallelepiped),
//it requires that a center be available. So, it's a separate calculation.
float minimumDistance = float.MaxValue;
for (int i = 0; i < triangleIndices.Count; i += 3)
{
System.Numerics.Vector3 v2 = vertices[triangleIndices[i]];
System.Numerics.Vector3 v3 = vertices[triangleIndices[i + 1]];
System.Numerics.Vector3 v4 = vertices[triangleIndices[i + 2]];
//This normal calculation creates a dependency on winding.
//It needs to be consistent with the SampleDirections triangle winding.
System.Numerics.Vector3 v2v3, v2v4;
Vector3Ex.Subtract(ref v3, ref v2, out v2v3);
Vector3Ex.Subtract(ref v4, ref v2, out v2v4);
System.Numerics.Vector3 normal;
Vector3Ex.Cross(ref v2v4, ref v2v3, out normal);
//Watch out: this could very easily be a degenerate triangle; the sampling approach tends to create them.
float lengthSquared = normal.LengthSquared();
if (lengthSquared > 1e-10f)
{
Vector3Ex.Divide(ref normal, (float)Math.Sqrt(lengthSquared), out normal);
}
else
{
continue;
}
System.Numerics.Vector3 fromCenterToPlane;
Vector3Ex.Subtract(ref v2, ref center, out fromCenterToPlane);
float distance;
Vector3Ex.Dot(ref normal, ref fromCenterToPlane, out distance);
if (distance < 0)
{
throw new ArgumentException("Invalid distance. Ensure the mesh is convex, has consistent winding, and contains the passed-in center.");
}
if (distance < minimumDistance)
{
minimumDistance = distance;
}
}
return(minimumDistance);
//Technically, we could also compute a maximum radius estimate...
//but that amounts to finding the furthest distance contained by the set of planes defined by the sampled extreme points and their associated sample directions.
//That's a trickier thing to compute quickly, and it's not all that important to make the estimate ultra tight.
}
void GetBuoyancyInformation(EntityCollidable collidable, out float submergedVolume, out System.Numerics.Vector3 submergedCenter)
{
BoundingBox entityBoundingBox;
RigidTransform localTransform;
RigidTransform.MultiplyByInverse(ref collidable.worldTransform, ref surfaceTransform, out localTransform);
collidable.Shape.GetBoundingBox(ref localTransform, out entityBoundingBox);
if (entityBoundingBox.Min.Y > 0)
{
//Fish out of the water. Don't need to do raycast tests on objects not at the boundary.
submergedVolume = 0;
submergedCenter = collidable.worldTransform.Position;
return;
}
if (entityBoundingBox.Max.Y < 0)
{
submergedVolume = collidable.entity.CollisionInformation.Shape.Volume;
submergedCenter = collidable.worldTransform.Position;
return;
}
System.Numerics.Vector3 origin, xSpacing, zSpacing;
float perColumnArea;
GetSamplingOrigin(ref entityBoundingBox, out xSpacing, out zSpacing, out perColumnArea, out origin);
float boundingBoxHeight = entityBoundingBox.Max.Y - entityBoundingBox.Min.Y;
float maxLength = -entityBoundingBox.Min.Y;
submergedCenter = new System.Numerics.Vector3();
submergedVolume = 0;
for (int i = 0; i < samplePointsPerDimension; i++)
{
for (int j = 0; j < samplePointsPerDimension; j++)
{
System.Numerics.Vector3 columnVolumeCenter;
float submergedHeight;
if ((submergedHeight = GetSubmergedHeight(collidable, maxLength, boundingBoxHeight, ref origin, ref xSpacing, ref zSpacing, i, j, out columnVolumeCenter)) > 0)
{
float columnVolume = submergedHeight * perColumnArea;
Vector3Ex.Multiply(ref columnVolumeCenter, columnVolume, out columnVolumeCenter);
Vector3Ex.Add(ref columnVolumeCenter, ref submergedCenter, out submergedCenter);
submergedVolume += columnVolume;
}
}
}
Vector3Ex.Divide(ref submergedCenter, submergedVolume, out submergedCenter);
//Pull the submerged center into world space before applying the force.
RigidTransform.Transform(ref submergedCenter, ref surfaceTransform, out submergedCenter);
}
bool TryDownCast(ref Ray ray, float length, out bool hasTraction, out SupportRayData supportRayData)
{
RayHit earliestHit;
Collidable earliestHitObject;
supportRayData = new SupportRayData();
hasTraction = false;
if (QueryManager.RayCast(ray, length, out earliestHit, out earliestHitObject))
{
float lengthSquared = earliestHit.Normal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon)
{
//Don't try to continue if the support ray is stuck in something.
return(false);
}
Vector3Ex.Divide(ref earliestHit.Normal, (float)Math.Sqrt(lengthSquared), out earliestHit.Normal);
//A collidable was hit! It's a support, but does it provide traction?
earliestHit.Normal.Normalize();
float dot;
Vector3Ex.Dot(ref ray.Direction, ref earliestHit.Normal, out dot);
if (dot < 0)
{
//Calibrate the normal so it always faces the same direction relative to the body.
Vector3Ex.Negate(ref earliestHit.Normal, out earliestHit.Normal);
dot = -dot;
}
//This down cast is only used for finding supports and traction, not for finding side contacts.
//If the detected normal is too steep, toss it out.
if (dot > ContactCategorizer.TractionThreshold)
{
//It has traction!
hasTraction = true;
supportRayData = new SupportRayData {
HitData = earliestHit, HitObject = earliestHitObject, HasTraction = true
};
}
else if (dot > ContactCategorizer.SupportThreshold)
{
supportRayData = new SupportRayData {
HitData = earliestHit, HitObject = earliestHitObject
}
}
;
else
{
return(false); //Too steep! Toss it out.
}
return(true);
}
return(false);
}
///<summary>
/// Computes an orientation and length from a line segment.
///</summary>
///<param name="start">Starting point of the line segment.</param>
///<param name="end">Endpoint of the line segment.</param>
///<param name="orientation">Orientation of a line that fits the line segment.</param>
///<param name="length">Length of the line segment.</param>
public static void GetCapsuleInformation(ref System.Numerics.Vector3 start, ref System.Numerics.Vector3 end, out System.Numerics.Quaternion orientation, out float length)
{
System.Numerics.Vector3 segmentDirection;
Vector3Ex.Subtract(ref end, ref start, out segmentDirection);
length = segmentDirection.Length();
if (length > 0)
{
Vector3Ex.Divide(ref segmentDirection, length, out segmentDirection);
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref segmentDirection, out orientation);
}
else
{
orientation = System.Numerics.Quaternion.Identity;
}
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = linearJacobianB = new Matrix3x3();
//There are two free axes and one restricted axis.
//The constraint attempts to keep the hinge axis attached to connection A and the twist axis attached to connection B perpendicular to each other.
//The restricted axis is the cross product between the twist and hinge axes.
System.Numerics.Vector3 worldTwistAxis, worldHingeAxis;
QuaternionEx.Transform(ref LocalHingeAxis, ref ConnectionA.Orientation, out worldHingeAxis);
QuaternionEx.Transform(ref LocalTwistAxis, ref ConnectionB.Orientation, out worldTwistAxis);
System.Numerics.Vector3 restrictedAxis;
Vector3Ex.Cross(ref worldHingeAxis, ref worldTwistAxis, out restrictedAxis);
//Attempt to normalize the restricted axis.
float lengthSquared = restrictedAxis.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref restrictedAxis, (float)Math.Sqrt(lengthSquared), out restrictedAxis);
}
else
{
restrictedAxis = new System.Numerics.Vector3();
}
angularJacobianA = new Matrix3x3
{
M11 = restrictedAxis.X,
M12 = restrictedAxis.Y,
M13 = restrictedAxis.Z,
};
Matrix3x3.Negate(ref angularJacobianA, out angularJacobianB);
float error;
Vector3Ex.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
error = (float)Math.Acos(MathHelper.Clamp(error, -1, 1)) - MathHelper.PiOver2;
velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * error, 0, 0);
}
void ComputeRestrictedAxes()
{
System.Numerics.Vector3 cross;
Vector3Ex.Cross(ref localLineDirection, ref Toolbox.UpVector, out cross);
float lengthSquared = cross.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref cross, (float)Math.Sqrt(lengthSquared), out localRestrictedAxis1);
}
else
{
//Oops! The direction is aligned with the up vector.
Vector3Ex.Cross(ref localLineDirection, ref Toolbox.RightVector, out cross);
Vector3Ex.Normalize(ref cross, out localRestrictedAxis1);
}
//Don't need to normalize this; cross product of two unit length perpendicular vectors.
Vector3Ex.Cross(ref localRestrictedAxis1, ref localLineDirection, out localRestrictedAxis2);
}
/// <summary>
/// Constructs a compound collidable containing only the specified subset of children.
/// </summary>
/// <param name="shape">Shape to base the compound collidable on.</param>
/// <param name="childIndices">Indices of child shapes from the CompoundShape to include in the compound collidable.</param>
/// <returns>Compound collidable containing only the specified subset of children.</returns>
public static CompoundCollidable CreatePartialCompoundCollidable(CompoundShape shape, IList <int> childIndices)
{
if (childIndices.Count == 0)
{
throw new ArgumentException("Cannot create a compound from zero shapes.");
}
CompoundCollidable compound = new CompoundCollidable();
System.Numerics.Vector3 center = new System.Numerics.Vector3();
float totalWeight = 0;
for (int i = 0; i < childIndices.Count; i++)
{
//Create and add the child object itself.
var entry = shape.shapes[childIndices[i]];
compound.children.Add(new CompoundChild(shape, entry.Shape.GetCollidableInstance(), childIndices[i]));
//Grab its entry to compute the center of mass of this subset.
System.Numerics.Vector3 toAdd;
Vector3Ex.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd);
Vector3Ex.Add(ref center, ref toAdd, out center);
totalWeight += entry.Weight;
}
if (totalWeight <= 0)
{
throw new ArgumentException("Compound has zero total weight; invalid configuration.");
}
Vector3Ex.Divide(ref center, totalWeight, out center);
//Our subset of the compound is not necessarily aligned with the shape's origin.
//By default, an object will rotate around the center of the collision shape.
//We can't modify the shape data itself since it could be shared, which leaves
//modifying the local position of the collidable.
//We have the subset position in shape space, so pull the collidable back into alignment
//with the origin.
//This approach matches the rest of the CompoundHelper's treatment of subsets.
compound.LocalPosition = -center;
//Recompute the hierarchy for the compound.
compound.hierarchy.Tree.Reconstruct(compound.children);
compound.Shape = shape;
return(compound);
}
/// <summary>
/// Computes contact data for two spheres.
/// </summary>
/// <param name="a">First sphere.</param>
/// <param name="b">Second sphere.</param>
/// <param name="positionA">Position of the first sphere.</param>
/// <param name="positionB">Position of the second sphere.</param>
/// <param name="contact">Contact data between the spheres, if any.</param>
/// <returns>Whether or not the spheres are touching.</returns>
public static bool AreSpheresColliding(SphereShape a, SphereShape b, ref System.Numerics.Vector3 positionA, ref System.Numerics.Vector3 positionB, out ContactData contact)
{
contact = new ContactData();
float radiusSum = a.collisionMargin + b.collisionMargin;
System.Numerics.Vector3 centerDifference;
Vector3Ex.Subtract(ref positionB, ref positionA, out centerDifference);
float centerDistance = centerDifference.LengthSquared();
if (centerDistance < (radiusSum + CollisionDetectionSettings.maximumContactDistance) * (radiusSum + CollisionDetectionSettings.maximumContactDistance))
{
//In collision!
if (radiusSum > Toolbox.Epsilon) //This would be weird, but it is still possible to cause a NaN.
{
Vector3Ex.Multiply(ref centerDifference, a.collisionMargin / (radiusSum), out contact.Position);
}
else
{
contact.Position = new System.Numerics.Vector3();
}
Vector3Ex.Add(ref contact.Position, ref positionA, out contact.Position);
centerDistance = (float)Math.Sqrt(centerDistance);
if (centerDistance > Toolbox.BigEpsilon)
{
Vector3Ex.Divide(ref centerDifference, centerDistance, out contact.Normal);
}
else
{
contact.Normal = Toolbox.UpVector;
}
contact.PenetrationDepth = radiusSum - centerDistance;
return(true);
}
return(false);
}
void ComputeConstrainedAxes()
{
System.Numerics.Vector3 worldAxisA = WorldFreeAxisA;
System.Numerics.Vector3 error = System.Numerics.Vector3.Cross(worldAxisA, WorldFreeAxisB);
float lengthSquared = error.LengthSquared();
System.Numerics.Vector3 worldConstrainedAxis1, worldConstrainedAxis2;
//Find the first constrained axis.
if (lengthSquared > Toolbox.Epsilon)
{
//The error direction can be used as the first axis!
Vector3Ex.Divide(ref error, (float)Math.Sqrt(lengthSquared), out worldConstrainedAxis1);
}
else
{
//There's not enough error for it to be a good constrained axis.
//We'll need to create the constrained axes arbitrarily.
Vector3Ex.Cross(ref Toolbox.UpVector, ref worldAxisA, out worldConstrainedAxis1);
lengthSquared = worldConstrainedAxis1.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//The up vector worked!
Vector3Ex.Divide(ref worldConstrainedAxis1, (float)Math.Sqrt(lengthSquared), out worldConstrainedAxis1);
}
else
{
//The up vector didn't work. Just try the right vector.
Vector3Ex.Cross(ref Toolbox.RightVector, ref worldAxisA, out worldConstrainedAxis1);
worldConstrainedAxis1.Normalize();
}
}
//Don't have to normalize the second constraint axis; it's the cross product of two perpendicular normalized vectors.
Vector3Ex.Cross(ref worldAxisA, ref worldConstrainedAxis1, out worldConstrainedAxis2);
localConstrainedAxis1 = QuaternionEx.Transform(worldConstrainedAxis1, QuaternionEx.Conjugate(ConnectionA.Orientation));
localConstrainedAxis2 = QuaternionEx.Transform(worldConstrainedAxis2, QuaternionEx.Conjugate(ConnectionA.Orientation));
}
/// <summary>
/// Do any necessary computations to prepare the constraint for this frame.
/// </summary>
/// <param name="dt">Simulation step length.</param>
public override void Update(float dt)
{
//Transform the axes into world space.
Matrix3x3.Transform(ref localHingeAxis, ref connectionA.orientationMatrix, out worldHingeAxis);
Matrix3x3.Transform(ref localTwistAxis, ref connectionB.orientationMatrix, out worldTwistAxis);
//****** VELOCITY BIAS ******//
Vector3Ex.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
//Compute the correction velocity.
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);
//Compute the jacobian
Vector3Ex.Cross(ref worldHingeAxis, ref worldTwistAxis, out jacobianA);
float length = jacobianA.LengthSquared();
if (length > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref jacobianA, (float)Math.Sqrt(length), out jacobianA);
}
else
{
jacobianA = new System.Numerics.Vector3();
}
jacobianB.X = -jacobianA.X;
jacobianB.Y = -jacobianA.Y;
jacobianB.Z = -jacobianA.Z;
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float entryA;
System.Numerics.Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3Ex.Dot(ref transformedAxis, ref jacobianA, out entryA);
}
else
{
entryA = 0;
}
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3Ex.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
{
entryB = 0;
}
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}
private bool DoExternalNear(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
System.Numerics.Vector3 closestA, closestB;
//Don't bother trying to do any clever caching. The continually transforming simplex makes it very rarely useful.
//TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle.
//If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin.
//This causes an instant-out, always. Not good!
//By giving the contributing simplex the average centroid, it has a better guess.
System.Numerics.Vector3 triangleCentroid;
Vector3Ex.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid);
Vector3Ex.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid);
Vector3Ex.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid);
var initialSimplex = new CachedSimplex {
State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid }
};
if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref initialSimplex, out closestA, out closestB))
{
state = CollisionState.Deep;
return(DoDeepContact(triangle, out contactList));
}
System.Numerics.Vector3 displacement;
Vector3Ex.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
float margin = convex.collisionMargin + triangle.collisionMargin;
contactList = new TinyStructList <ContactData>();
if (distanceSquared < margin * margin)
{
//Try to generate a contact.
var contact = new ContactData();
//Determine if the normal points in the appropriate direction given the sidedness of the triangle.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
System.Numerics.Vector3 triangleNormal, ab, ac;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3Ex.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3Ex.Dot(ref triangleNormal, ref displacement, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
return(false);
}
if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
return(false);
}
}
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //This can be zero! It would cause a NaN if unprotected.
{
Vector3Ex.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB
}
else
{
contact.Position = new System.Numerics.Vector3();
}
Vector3Ex.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3Ex.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
contactList.Add(ref contact);
TryToEscape(triangle, ref contact.Position);
return(true);
}
//Too far to make a contact- move back to separation.
state = CollisionState.ExternalSeparated;
return(false);
}
private bool DoShallowContact(out ContactData contact)
{
System.Numerics.Vector3 closestA, closestB;
//RigidTransform transform = RigidTransform.Identity;
//System.Numerics.Vector3 closestAnew, closestBnew;
//CachedSimplex cachedTest = cachedSimplex;
//bool intersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, ref cachedTest, out closestAnew, out closestBnew);
////bool otherIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestA, out closestB);
//bool otherIntersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, out closestA, out closestB);
//System.Numerics.Vector3 closestAold, closestBold;
//bool oldIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestAold, out closestBold);
//if (otherIntersecting != intersecting || (!otherIntersecting && !intersecting &&
// Vector3Ex.DistanceSquared(closestAnew, closestBnew) - Vector3Ex.DistanceSquared(closestA, closestB) > .0001f &&
// (Vector3Ex.DistanceSquared(closestA, closestAnew) > .0001f ||
// Vector3Ex.DistanceSquared(closestB, closestBnew) > .0001f)))// ||
// //Math.Abs(Vector3Ex.Dot(closestB - closestA, closestBnew - closestAnew) - Vector3Ex.Dot(closestB - closestA, closestB - closestA)) > Toolbox.Epsilon)))
// Debug.WriteLine("Break.");
//System.Numerics.Vector3 sub;
//Vector3Ex.Subtract(ref closestA, ref closestB, out sub);
//if (sub.LengthSquared() < Toolbox.Epsilon)
if (UseSimplexCaching)
{
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB);
}
else
{
//The initialization of the pair creates a pretty decent simplex to start from.
//Just don't try to update it.
CachedSimplex preInitializedSimplex = cachedSimplex;
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB);
}
System.Numerics.Vector3 displacement;
Vector3Ex.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
if (distanceSquared < Toolbox.Epsilon)
{
state = CollisionState.DeepContact;
return(DoDeepContact(out contact));
}
localDirection = displacement; //Use this as the direction for future deep contacts.
float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin;
if (distanceSquared < margin * margin)
{
//Generate a contact.
contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //Avoid a NaN!
{
Vector3Ex.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin, out contact.Position); //t * AB
}
else
{
contact.Position = new System.Numerics.Vector3();
}
Vector3Ex.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3Ex.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
return(true);
}
//Too shallow to make a contact- move back to separation.
state = CollisionState.Separated;
contact = new ContactData();
return(false);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
System.Numerics.Vector3 axisA, axisB;
QuaternionEx.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
QuaternionEx.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
System.Numerics.Vector3 twistMeasureAxisA, twistMeasureAxisB;
QuaternionEx.Transform(ref LocalMeasurementAxisA, ref ConnectionA.Orientation, out twistMeasureAxisA);
QuaternionEx.Transform(ref LocalMeasurementAxisB, ref ConnectionB.Orientation, out twistMeasureAxisB);
//Compute the shortest rotation to bring axisB into alignment with axisA.
System.Numerics.Quaternion alignmentRotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref axisB, ref axisA, out alignmentRotation);
//Transform the measurement axis on B by the alignment quaternion.
QuaternionEx.Transform(ref twistMeasureAxisB, ref alignmentRotation, out twistMeasureAxisB);
//We can now compare the angle between the twist axes.
float angle;
Vector3Ex.Dot(ref twistMeasureAxisA, ref twistMeasureAxisB, out angle);
angle = (float)Math.Acos(MathHelper.Clamp(angle, -1, 1));
//Compute the bias based upon the error.
if (angle > maximumAngle)
{
velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * (angle - maximumAngle), 0, 0);
}
else //If the constraint isn't violated, set up the velocity bias to allow a 'speculative' limit.
{
velocityBias = new System.Numerics.Vector3(angle - maximumAngle, 0, 0);
}
//We can't just use the axes directly as jacobians. Consider 'cranking' one object around the other.
System.Numerics.Vector3 jacobian;
Vector3Ex.Add(ref axisA, ref axisB, out jacobian);
float lengthSquared = jacobian.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref jacobian, (float)Math.Sqrt(lengthSquared), out jacobian);
}
else
{
//The constraint is in an invalid configuration. Just ignore it.
jacobian = new System.Numerics.Vector3();
}
//In addition to the absolute angle value, we need to know which side of the limit we're hitting.
//The jacobian will be negated on one side. This is because limits can only 'push' in one direction;
//if we didn't flip the direction of the jacobian, it would be trying to push the same direction on both ends of the limit.
//One side would end up doing nothing!
System.Numerics.Vector3 cross;
Vector3Ex.Cross(ref twistMeasureAxisA, ref twistMeasureAxisB, out cross);
float limitSide;
Vector3Ex.Dot(ref cross, ref axisA, out limitSide);
//Negate the jacobian based on what side of the limit we're on.
if (limitSide < 0)
{
Vector3Ex.Negate(ref jacobian, out jacobian);
}
angularJacobianA = new Matrix3x3 {
M11 = jacobian.X, M12 = jacobian.Y, M13 = jacobian.Z
};
angularJacobianB = new Matrix3x3 {
M11 = -jacobian.X, M12 = -jacobian.Y, M13 = -jacobian.Z
};
}
private bool DoDeepContact(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
//Find the origin to triangle center offset.
System.Numerics.Vector3 center;
Vector3Ex.Add(ref triangle.vA, ref triangle.vB, out center);
Vector3Ex.Add(ref center, ref triangle.vC, out center);
Vector3Ex.Multiply(ref center, 1f / 3f, out center);
ContactData contact;
contactList = new TinyStructList <ContactData>();
if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity))
{
float dot;
System.Numerics.Vector3 triangleNormal, ab, ac;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3Ex.Cross(ref ab, ref ac, out triangleNormal);
float lengthSquared = triangleNormal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon * .01f)
{
//Degenerate triangle! That's no good.
//Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
}
else
{
//Normalize the normal.
Vector3Ex.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal);
//TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast.
//Find the edge directions that will be tested with MPR.
System.Numerics.Vector3 AO, BO, CO;
System.Numerics.Vector3 AB, BC, CA;
Vector3Ex.Subtract(ref center, ref triangle.vA, out AO);
Vector3Ex.Subtract(ref center, ref triangle.vB, out BO);
Vector3Ex.Subtract(ref center, ref triangle.vC, out CO);
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vB, out BC);
Vector3Ex.Subtract(ref triangle.vA, ref triangle.vC, out CA);
//We don't have to worry about degenerate triangles here because we've already handled that possibility above.
System.Numerics.Vector3 ABnormal, BCnormal, CAnormal;
//Project the center onto the edge to find the direction from the center to the edge AB.
Vector3Ex.Dot(ref AO, ref AB, out dot);
Vector3Ex.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal);
Vector3Ex.Subtract(ref AO, ref ABnormal, out ABnormal);
ABnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3Ex.Dot(ref BO, ref BC, out dot);
Vector3Ex.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal);
Vector3Ex.Subtract(ref BO, ref BCnormal, out BCnormal);
BCnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3Ex.Dot(ref CO, ref CA, out dot);
Vector3Ex.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal);
Vector3Ex.Subtract(ref CO, ref CAnormal, out CAnormal);
CAnormal.Normalize();
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal, out contact.PenetrationDepth, out contact.Normal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3Ex.Dot(ref triangleNormal, ref contact.Normal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
System.Numerics.Vector3 previousNormal = contact.Normal;
Vector3Ex.Dot(ref contact.Normal, ref triangleNormal, out dot);
System.Numerics.Vector3 p;
Vector3Ex.Multiply(ref contact.Normal, dot, out p);
Vector3Ex.Subtract(ref contact.Normal, ref p, out contact.Normal);
float length = contact.Normal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3Ex.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
Vector3Ex.Dot(ref contact.Normal, ref previousNormal, out dot);
contact.PenetrationDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new System.Numerics.Vector3();
}
}
System.Numerics.Vector3 candidateNormal;
float candidateDepth;
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3Ex.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
System.Numerics.Vector3 previousNormal = candidateNormal;
Vector3Ex.Dot(ref candidateNormal, ref triangleNormal, out dot);
System.Numerics.Vector3 p;
Vector3Ex.Multiply(ref candidateNormal, dot, out p);
Vector3Ex.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3Ex.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3Ex.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new System.Numerics.Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3Ex.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
System.Numerics.Vector3 previousNormal = candidateNormal;
Vector3Ex.Dot(ref candidateNormal, ref triangleNormal, out dot);
System.Numerics.Vector3 p;
Vector3Ex.Multiply(ref candidateNormal, dot, out p);
Vector3Ex.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3Ex.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3Ex.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new System.Numerics.Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
//Try the depth along the positive triangle normal.
//If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Clockwise)
{
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
//Try the depth along the negative triangle normal.
//If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Counterclockwise)
{
Vector3Ex.Negate(ref triangleNormal, out triangleNormal);
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
}
MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
//It's possible for the normal to still face the 'wrong' direction according to one sided triangles.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3Ex.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (dot < 0)
{
//Skip the add process.
goto InnerSphere;
}
}
contact.Id = -1;
if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin)
{
state = CollisionState.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
}
contactList.Add(ref contact);
}
InnerSphere:
if (TryInnerSphereContact(triangle, out contact))
{
contactList.Add(ref contact);
}
if (contactList.Count > 0)
{
return(true);
}
state = CollisionState.ExternalSeparated;
return(false);
}
private bool TryInnerSphereContact(TriangleShape triangle, out ContactData contact)
{
System.Numerics.Vector3 closestPoint;
Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint);
float length = closestPoint.LengthSquared();
float minimumRadius = convex.MinimumRadius * (MotionSettings.CoreShapeScaling + .01f);
if (length < minimumRadius * minimumRadius)
{
System.Numerics.Vector3 triangleNormal, ab, ac;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3Ex.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3Ex.Dot(ref closestPoint, ref triangleNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
contact = new ContactData();
return(false);
}
length = (float)Math.Sqrt(length);
contact.Position = closestPoint;
if (length > Toolbox.Epsilon) //Watch out for NaN's!
{
Vector3Ex.Divide(ref closestPoint, length, out contact.Normal);
}
else
{
//The direction is undefined. Use the triangle's normal.
//One sided triangles can only face in the appropriate direction.
float normalLength = triangleNormal.LengthSquared();
if (triangleNormal.LengthSquared() > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref triangleNormal, (float)Math.Sqrt(normalLength), out triangleNormal);
if (triangle.sidedness == TriangleSidedness.Clockwise)
{
contact.Normal = triangleNormal;
}
else
{
Vector3Ex.Negate(ref triangleNormal, out contact.Normal);
}
}
else
{
//Degenerate triangle!
contact = new ContactData();
return(false);
}
}
//Compute the actual depth of the contact.
//This is conservative; the minimum radius is guaranteed to be no larger than the shape itself.
//But that's ok- this is strictly a deep contact protection scheme. Other contacts will make the objects separate.
contact.PenetrationDepth = convex.MinimumRadius - length;
contact.Id = -1;
return(true);
}
contact = new ContactData();
return(false);
}
protected override void ProcessCandidates(ref QuickList <ContactData> candidates)
{
if (candidates.Count == 0 && parentContactCount == 0 && Mesh.Shape.solidity == MobileMeshSolidity.Solid)
{
//If there's no new contacts on the mesh and it's supposed to be a solid,
//then we must check the convex for containment within the shell.
//We already know that it's not on the shell, meaning that the shape is either
//far enough away outside the shell that there's no contact (and we're done),
//or it's far enough inside the shell that the triangles cannot create contacts.
//To find out which it is, raycast against the shell.
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out orientation);
Ray ray;
Vector3Ex.Subtract(ref convex.worldTransform.Position, ref mesh.worldTransform.Position, out ray.Position);
Matrix3x3.TransformTranspose(ref ray.Position, ref orientation, out ray.Position);
//Cast from the current position back to the previous position.
Vector3Ex.Subtract(ref lastValidConvexPosition, ref ray.Position, out ray.Direction);
float rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//The object may not have moved enough to normalize properly. If so, choose something arbitrary.
//Try the direction from the center of the object to the convex's position.
ray.Direction = ray.Position;
rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//This is unlikely; just pick something completely arbitrary then.
ray.Direction = Vector3Ex.Up;
rayDirectionLength = 1;
}
}
Vector3Ex.Divide(ref ray.Direction, (float)Math.Sqrt(rayDirectionLength), out ray.Direction);
RayHit hit;
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
ContactData newContact = new ContactData {
Id = 2
};
//Give it a special id so that we know that it came from the inside.
Matrix3x3.Transform(ref ray.Position, ref orientation, out newContact.Position);
Vector3Ex.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position);
newContact.Normal = hit.Normal;
newContact.Normal.Normalize();
float factor;
Vector3Ex.Dot(ref ray.Direction, ref newContact.Normal, out factor);
newContact.PenetrationDepth = -factor * hit.T + convex.Shape.MinimumRadius;
Matrix3x3.Transform(ref newContact.Normal, ref orientation, out newContact.Normal);
newContact.Validate();
//Do not yet create a new contact. Check to see if an 'inner contact' with id == 2 already exists.
bool addContact = true;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new System.Numerics.Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
addContact = false;
break;
}
}
if (addContact && contacts.Count == 0)
{
Add(ref newContact);
}
previousDepth = newContact.PenetrationDepth;
}
else
{
//It's possible that we had a false negative. The previous frame may have been in deep intersection, and this frame just failed to come to the same conclusion.
//If we set the target location to the current location, the object will never escape the mesh. Instead, only do that if two frames agree that we are no longer colliding.
if (previousDepth > 0)
{
//We're not touching the mesh.
lastValidConvexPosition = ray.Position;
}
previousDepth = 0;
}
}
}
void CorrectContacts()
{
//Go through the contacts associated with the character.
//If the contact is at the bottom of the character, regardless of its normal, take a closer look.
//If the direction from the closest point on the inner cylinder to the contact position has traction
//and the contact's normal does not, then replace the contact normal with the offset direction.
//This is necessary because various convex pair manifolds use persistent manifolds.
//Contacts in these persistent manifolds can live too long for the character to behave perfectly
//when going over (usually tiny) steps.
System.Numerics.Vector3 downDirection = Body.OrientationMatrix.Down;
System.Numerics.Vector3 position = Body.Position;
float margin = Body.CollisionInformation.Shape.CollisionMargin;
float minimumHeight = Body.Height * .5f - margin;
float coreRadius = Body.Radius - margin;
float coreRadiusSquared = coreRadius * coreRadius;
foreach (var pair in Body.CollisionInformation.Pairs)
{
foreach (var contactData in pair.Contacts)
{
var contact = contactData.Contact;
float dot;
//Check to see if the contact position is at the bottom of the character.
System.Numerics.Vector3 offset = contact.Position - Body.Position;
Vector3Ex.Dot(ref offset, ref downDirection, out dot);
if (dot > minimumHeight)
{
//It is a 'bottom' contact!
//So, compute the offset from the inner cylinder to the contact.
//To do this, compute the closest point on the inner cylinder.
//Since we know it's on the bottom, all we need is to compute the horizontal offset.
Vector3Ex.Dot(ref offset, ref downDirection, out dot);
System.Numerics.Vector3 horizontalOffset;
Vector3Ex.Multiply(ref downDirection, dot, out horizontalOffset);
Vector3Ex.Subtract(ref offset, ref horizontalOffset, out horizontalOffset);
float length = horizontalOffset.LengthSquared();
if (length > coreRadiusSquared)
{
//It's beyond the edge of the cylinder; clamp it.
Vector3Ex.Multiply(ref horizontalOffset, coreRadius / (float)Math.Sqrt(length), out horizontalOffset);
}
//It's on the bottom, so add the bottom height.
System.Numerics.Vector3 closestPointOnCylinder;
Vector3Ex.Multiply(ref downDirection, minimumHeight, out closestPointOnCylinder);
Vector3Ex.Add(ref closestPointOnCylinder, ref horizontalOffset, out closestPointOnCylinder);
Vector3Ex.Add(ref closestPointOnCylinder, ref position, out closestPointOnCylinder);
//Compute the offset from the cylinder to the offset.
System.Numerics.Vector3 offsetDirection;
Vector3Ex.Subtract(ref contact.Position, ref closestPointOnCylinder, out offsetDirection);
length = offsetDirection.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Normalize the offset.
Vector3Ex.Divide(ref offsetDirection, (float)Math.Sqrt(length), out offsetDirection);
}
else
{
continue; //If there's no offset, it's really deep and correcting this contact might be a bad idea.
}
Vector3Ex.Dot(ref offsetDirection, ref downDirection, out dot);
float dotOriginal;
Vector3Ex.Dot(ref contact.Normal, ref downDirection, out dotOriginal);
if (dot > Math.Abs(dotOriginal)) //if the new offsetDirection normal is less steep than the original slope...
{
//Then use it!
Vector3Ex.Dot(ref offsetDirection, ref contact.Normal, out dot);
if (dot < 0)
{
//Don't flip the normal relative to the contact normal. That would be bad!
Vector3Ex.Negate(ref offsetDirection, out offsetDirection);
dot = -dot;
}
//Update the contact data using the corrected information.
//The penetration depth is conservatively updated; it will be less than or equal to the 'true' depth in this direction.
contact.PenetrationDepth *= dot;
contact.Normal = offsetDirection;
}
}
}
}
}
private bool DoPlaneTest(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
//Find closest point between object and plane.
System.Numerics.Vector3 reverseNormal;
System.Numerics.Vector3 ab, ac;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3Ex.Cross(ref ac, ref ab, out reverseNormal);
//Convex position dot normal is ALWAYS zero. The thing to look at is the plane's 'd'.
//If the distance along the normal is positive, then the convex is 'behind' that normal.
float dotA;
Vector3Ex.Dot(ref triangle.vA, ref reverseNormal, out dotA);
contactList = new TinyStructList <ContactData>();
switch (triangle.sidedness)
{
case TriangleSidedness.DoubleSided:
if (dotA < 0)
{
//The reverse normal is pointing towards the convex.
//It needs to point away from the convex so that the direction
//will get the proper extreme point.
Vector3Ex.Negate(ref reverseNormal, out reverseNormal);
dotA = -dotA;
}
break;
case TriangleSidedness.Clockwise:
//if (dotA < 0)
//{
// //The reverse normal is pointing towards the convex.
// return false;
//}
break;
case TriangleSidedness.Counterclockwise:
//if (dotA > 0)
//{
// //The reverse normal is pointing away from the convex.
// return false;
//}
//The reverse normal is pointing towards the convex.
//It needs to point away from the convex so that the direction
//will get the proper extreme point.
Vector3Ex.Negate(ref reverseNormal, out reverseNormal);
dotA = -dotA;
break;
}
System.Numerics.Vector3 extremePoint;
convex.GetLocalExtremePointWithoutMargin(ref reverseNormal, out extremePoint);
//See if the extreme point is within the face or not.
//It might seem like the easy "depth" test should come first, since a barycentric
//calculation takes a bit more time. However, transferring from plane to depth is 'rare'
//(like all transitions), and putting this test here is logically closer to its requirements'
//computation.
if (GetVoronoiRegion(triangle, ref extremePoint) != VoronoiRegion.ABC)
{
state = CollisionState.ExternalSeparated;
return(DoExternalSeparated(triangle, out contactList));
}
float dotE;
Vector3Ex.Dot(ref extremePoint, ref reverseNormal, out dotE);
float t = (dotA - dotE) / reverseNormal.LengthSquared();
System.Numerics.Vector3 offset;
Vector3Ex.Multiply(ref reverseNormal, t, out offset);
//Compare the distance from the plane to the convex object.
float distanceSquared = offset.LengthSquared();
float marginSum = triangle.collisionMargin + convex.collisionMargin;
//TODO: Could just normalize early and avoid computing point plane before it's necessary.
//Exposes a sqrt but...
if (t <= 0 || distanceSquared < marginSum * marginSum)
{
//The convex object is in the margin of the plane.
//All that's left is to create the contact.
var contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (marginSum > Toolbox.Epsilon) //This can be zero! It would cause a NaN is unprotected.
{
Vector3Ex.Multiply(ref offset, convex.collisionMargin / marginSum, out contact.Position); //t * AB
}
else
{
contact.Position = new System.Numerics.Vector3();
}
Vector3Ex.Add(ref extremePoint, ref contact.Position, out contact.Position); //A + t * AB.
float normalLength = reverseNormal.Length();
Vector3Ex.Divide(ref reverseNormal, normalLength, out contact.Normal);
float distance = normalLength * t;
contact.PenetrationDepth = marginSum - distance;
if (contact.PenetrationDepth > marginSum)
{
//Check to see if the inner sphere is touching the plane.
//This does not override other tests; there can be more than one contact from a single triangle.
ContactData alternateContact;
if (TryInnerSphereContact(triangle, out alternateContact))// && alternateContact.PenetrationDepth > contact.PenetrationDepth)
{
contactList.Add(ref alternateContact);
}
//The convex object is stuck deep in the plane!
//The most problematic case for this is when
//an object is right on top of a cliff.
//The lower, vertical triangle may occasionally detect
//a contact with the object, but would compute an extremely
//deep depth if the normal plane test was used.
//Verify that the depth is correct by trying another approach.
CollisionState previousState = state;
state = CollisionState.ExternalNear;
TinyStructList <ContactData> alternateContacts;
if (DoExternalNear(triangle, out alternateContacts))
{
alternateContacts.Get(0, out alternateContact);
if (alternateContact.PenetrationDepth + .01f < contact.PenetrationDepth) //Bias against the subtest's result, since the plane version will probably have a better position.
{
//It WAS a bad contact.
contactList.Add(ref alternateContact);
//DoDeepContact (which can be called from within DoExternalNear) can generate two contacts, but the second contact would just be an inner sphere (which we already generated).
//DoExternalNear can only generate one contact. So we only need the first contact!
//TODO: This is a fairly fragile connection between the two stages. Consider robustifying. (Also, the TryInnerSphereContact is done twice! This process is very rare for marginful pairs, though)
}
else
{
//Well, it really is just that deep.
contactList.Add(ref contact);
state = previousState;
}
}
else
{
//If the external near test finds that there was no collision at all,
//just return to plane testing. If the point turns up outside the face region
//next time, the system will adapt.
state = previousState;
return(false);
}
}
else
{
contactList.Add(ref contact);
}
return(true);
}
return(false);
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
bool isTryingToMove = movementDirection3d.LengthSquared() > 0;
if (!isTryingToMove)
{
TargetSpeed = 0;
}
maxForce = MaximumForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
System.Numerics.Vector3 downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
System.Numerics.Vector3 velocityDirection;
System.Numerics.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.
System.Numerics.Vector3 lineStart = movementDirection3d;
System.Numerics.Vector3 lineEnd;
Vector3Ex.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.
Vector3Ex.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;
Vector3Ex.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
System.Numerics.Vector3 toRemove;
Vector3Ex.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3Ex.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3Ex.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3Ex.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3Ex.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.
}
}
Vector3Ex.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.
Vector3Ex.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.
System.Numerics.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.
Vector3Ex.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3Ex.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new System.Numerics.Vector3();
angularJacobianB2 = new System.Numerics.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 = System.Numerics.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 = TargetSpeed;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
System.Numerics.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);
Vector3Ex.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3Ex.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3Ex.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)
{
Vector3Ex.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)
{
System.Numerics.Vector3 targetPosition;
Vector3Ex.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
System.Numerics.Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
System.Numerics.Vector3 error;
Vector3Ex.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)
{
Vector3Ex.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new System.Numerics.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!
Vector3Ex.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3Ex.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2Ex.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new System.Numerics.Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
///<summary>
/// Tests if a box and sphere are colliding.
///</summary>
///<param name="box">Box to test.</param>
///<param name="sphere">Sphere to test.</param>
///<param name="boxTransform">Transform to apply to the box.</param>
///<param name="spherePosition">Transform to apply to the sphere.</param>
///<param name="contact">Contact point between the shapes, if any.</param>
///<returns>Whether or not the shapes were colliding.</returns>
public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref System.Numerics.Vector3 spherePosition, out ContactData contact)
{
contact = new ContactData();
System.Numerics.Vector3 localPosition;
RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition);
#if !WINDOWS
System.Numerics.Vector3 localClosestPoint = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 localClosestPoint;
#endif
localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth);
localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight);
localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength);
RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position);
System.Numerics.Vector3 offset;
Vector3Ex.Subtract(ref spherePosition, ref contact.Position, out offset);
float offsetLength = offset.LengthSquared();
if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance))
{
return(false);
}
//Colliding.
if (offsetLength > Toolbox.Epsilon)
{
offsetLength = (float)Math.Sqrt(offsetLength);
//Outside of the box.
Vector3Ex.Divide(ref offset, offsetLength, out contact.Normal);
contact.PenetrationDepth = sphere.collisionMargin - offsetLength;
}
else
{
//Inside of the box.
System.Numerics.Vector3 penetrationDepths;
penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X;
penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y;
penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z;
if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector;
contact.PenetrationDepth = penetrationDepths.X;
}
else if (penetrationDepths.Y < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector;
contact.PenetrationDepth = penetrationDepths.Y;
}
else
{
contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector;
contact.PenetrationDepth = penetrationDepths.X;
}
contact.PenetrationDepth += sphere.collisionMargin;
QuaternionEx.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal);
}
return(true);
}
//Relies on the triangle being located in the local space of the convex object. The convex transform is used to transform the
//contact points back from the convex's local space into world space.
///<summary>
/// Generates a contact between the triangle and convex.
///</summary>
///<param name="contactList">Contact between the shapes, if any.</param>
///<returns>Whether or not the shapes are colliding.</returns>
public override bool GenerateContactCandidates(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
contactList = new TinyStructList <ContactData>();
System.Numerics.Vector3 ab, ac;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out ac);
System.Numerics.Vector3 triangleNormal;
Vector3Ex.Cross(ref ab, ref ac, out triangleNormal);
if (triangleNormal.LengthSquared() < Toolbox.Epsilon * .01f)
{
//If the triangle is degenerate, use the offset between its center and the sphere.
Vector3Ex.Add(ref triangle.vA, ref triangle.vB, out triangleNormal);
Vector3Ex.Add(ref triangleNormal, ref triangle.vC, out triangleNormal);
Vector3Ex.Multiply(ref triangleNormal, 1 / 3f, out triangleNormal);
if (triangleNormal.LengthSquared() < Toolbox.Epsilon * .01f)
{
triangleNormal = Toolbox.UpVector; //Alrighty then! Pick a random direction.
}
}
float dot;
Vector3Ex.Dot(ref triangleNormal, ref triangle.vA, out dot);
switch (triangle.sidedness)
{
case TriangleSidedness.DoubleSided:
if (dot < 0)
{
Vector3Ex.Negate(ref triangleNormal, out triangleNormal); //Normal must face outward.
}
break;
case TriangleSidedness.Clockwise:
if (dot > 0)
{
return(false); //Wrong side, can't have a contact pointing in a reasonable direction.
}
break;
case TriangleSidedness.Counterclockwise:
if (dot < 0)
{
return(false); //Wrong side, can't have a contact pointing in a reasonable direction.
}
break;
}
System.Numerics.Vector3 closestPoint;
//Could optimize this process a bit. The 'point' being compared is always zero. Additionally, since the triangle normal is available,
//there is a little extra possible optimization.
lastRegion = Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint);
float lengthSquared = closestPoint.LengthSquared();
float marginSum = triangle.collisionMargin + sphere.collisionMargin;
if (lengthSquared <= marginSum * marginSum)
{
var contact = new ContactData();
if (lengthSquared < Toolbox.Epsilon)
{
//Super close to the triangle. Normalizing would be dangerous.
Vector3Ex.Negate(ref triangleNormal, out contact.Normal);
contact.Normal.Normalize();
contact.PenetrationDepth = marginSum;
contactList.Add(ref contact);
return(true);
}
lengthSquared = (float)Math.Sqrt(lengthSquared);
Vector3Ex.Divide(ref closestPoint, lengthSquared, out contact.Normal);
contact.PenetrationDepth = marginSum - lengthSquared;
contact.Position = closestPoint;
contactList.Add(ref contact);
return(true);
}
return(false);
}
/// <summary>
/// Removes a child from a compound collidable.
/// </summary>
/// <param name="compound">Compound collidable to remove a child from.</param>
/// <param name="removalPredicate">Callback which analyzes a child and determines if it should be removed from the compound.</param>
/// <param name="childContributions">Distribution contributions from all shapes in the compound shape. This can include shapes which are not represented in the compound.</param>
/// <param name="distributionInfo">Distribution information of the new compound.</param>
/// <param name="weight">Total weight of the new compound.</param>
/// <param name="removedWeight">Weight removed from the compound.</param>
/// <param name="removedCenter">Center of the chunk removed from the compound.</param>
/// <returns>Whether or not any removal took place.</returns>
public static bool RemoveChildFromCompound(CompoundCollidable compound, Func <CompoundChild, bool> removalPredicate, IList <ShapeDistributionInformation> childContributions,
out ShapeDistributionInformation distributionInfo, out float weight, out float removedWeight, out System.Numerics.Vector3 removedCenter)
{
bool removalOccurred = false;
removedWeight = 0;
removedCenter = new System.Numerics.Vector3();
for (int i = compound.children.Count - 1; i >= 0; i--)
{
//The shape doesn't change during this process. The entity could, though.
//All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
var child = compound.children.Elements[i];
if (removalPredicate(child))
{
removalOccurred = true;
var entry = child.Entry;
removedWeight += entry.Weight;
System.Numerics.Vector3 toAdd;
Vector3Ex.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd);
Vector3Ex.Add(ref removedCenter, ref toAdd, out removedCenter);
//The child event handler must be unhooked from the compound.
child.CollisionInformation.events.Parent = null;
compound.children.FastRemoveAt(i);
}
}
if (!removalOccurred)
{
//No removal occurred, so we cannot proceed.
distributionInfo = new ShapeDistributionInformation();
weight = 0;
return(false);
}
if (removedWeight > 0)
{
Vector3Ex.Divide(ref removedCenter, removedWeight, out removedCenter);
}
//Compute the contributions from the original shape to the new form of the original collidable.
distributionInfo = new ShapeDistributionInformation();
weight = 0;
for (int i = compound.children.Count - 1; i >= 0; i--)
{
var child = compound.children.Elements[i];
var entry = child.Entry;
var contribution = childContributions[child.shapeIndex];
Vector3Ex.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center);
Vector3Ex.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center);
Vector3Ex.Add(ref contribution.Center, ref distributionInfo.Center, out distributionInfo.Center);
distributionInfo.Volume += contribution.Volume;
weight += entry.Weight;
}
//Average the center out.
Vector3Ex.Divide(ref distributionInfo.Center, weight, out distributionInfo.Center);
//Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
//That is not technically the center of the new collidable- distributionInfo.Center is.
//Offset the child collidables by -distributionInfo.Center using their local offset.
System.Numerics.Vector3 offset;
Vector3Ex.Negate(ref distributionInfo.Center, out offset);
//Compute the unscaled inertia tensor.
for (int i = compound.children.Count - 1; i >= 0; i--)
{
var child = compound.children.Elements[i];
var entry = child.Entry;
System.Numerics.Vector3 transformedOffset;
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
QuaternionEx.Transform(ref offset, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
var contribution = childContributions[child.shapeIndex];
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfo.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution);
//Vector3Ex.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position);
Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfo.VolumeDistribution, out distributionInfo.VolumeDistribution);
}
//Normalize the volume distribution.
Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, 1 / weight, out distributionInfo.VolumeDistribution);
//Update the hierarchies of the compounds.
//TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
compound.hierarchy.Tree.Reconstruct(compound.children);
return(true);
}
/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//Put the ray into local space.
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3Ex.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
QuaternionEx.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
//Check for containment.
if (localRay.Position.Y >= -halfHeight && localRay.Position.Y <= halfHeight && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= radius * radius)
{
//It's inside!
hit.T = 0;
hit.Location = localRay.Position;
hit.Normal = new System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new System.Numerics.Vector3();
}
//Pull the hit into world space.
QuaternionEx.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//Project the ray direction onto the plane where the cylinder is a circle.
//The projected ray is then tested against the circle to compute the time of impact.
//That time of impact is used to compute the 3d hit location.
System.Numerics.Vector2 planeDirection = new System.Numerics.Vector2(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared();
if (planeDirectionLengthSquared < Toolbox.Epsilon)
{
//The ray is nearly parallel with the axis.
//Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside
//and won't hit the sides.
if (localRay.Position.Y > halfHeight)
{
goto upperTest;
}
if (localRay.Position.Y < -halfHeight)
{
goto lowerTest;
}
hit = new RayHit();
return(false);
}
System.Numerics.Vector2 planeOrigin = new System.Numerics.Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2Ex.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
System.Numerics.Vector2 closestPoint;
Vector2Ex.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2Ex.Add(ref planeOrigin, ref closestPoint, out closestPoint);
//How close does the ray come to the circle?
float squaredDistance = closestPoint.LengthSquared();
if (squaredDistance > radius * radius)
{
//It's too far! The ray cannot possibly hit the capsule.
hit = new RayHit();
return(false);
}
//With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis.
float backwardsDistance = radius * (float)Math.Sqrt(1 - squaredDistance / (radius * radius));
float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);
hit.T = closestToCenterT - tOffset;
//Compute the impact point on the infinite cylinder in 3d local space.
Vector3Ex.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref localRay.Position, out hit.Location);
//Is it intersecting the cylindrical portion of the capsule?
if (hit.Location.Y <= halfHeight && hit.Location.Y >= -halfHeight && hit.T < maximumLength)
{
//Yup!
hit.Normal = new System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new System.Numerics.Vector3();
}
//Pull the hit into world space.
QuaternionEx.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
if (hit.Location.Y < halfHeight)
{
goto lowerTest;
}
upperTest:
//Nope! It may be intersecting the ends of the cylinder though.
//We're above the cylinder, so cast a ray against the upper cap.
if (localRay.Direction.Y > -1e-9)
{
//Can't hit the upper cap if the ray isn't pointing down.
hit = new RayHit();
return(false);
}
float t = (halfHeight - localRay.Position.Y) / localRay.Direction.Y;
System.Numerics.Vector3 planeIntersection;
Vector3Ex.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3Ex.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
QuaternionEx.Transform(ref Toolbox.UpVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
lowerTest:
//Is it intersecting the bottom cap?
if (localRay.Direction.Y < 1e-9)
{
//Can't hit the bottom cap if the ray isn't pointing up.
hit = new RayHit();
return(false);
}
t = (-halfHeight - localRay.Position.Y) / localRay.Direction.Y;
Vector3Ex.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3Ex.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
QuaternionEx.Transform(ref Toolbox.DownVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
}
/// <summary>
/// Computes representative support information based on the character's current traction contacts, support contacts, and ray contacts.
/// </summary>
/// <param name="down">Down direction of the character.</param>
private void UpdateSupportData(ref System.Numerics.Vector3 down)
{
//Choose which set of contacts to use.
RawList <CharacterContact> contacts;
if (tractionContacts.Count > 0)
{
contacts = tractionContacts;
}
else if (supportContacts.Count > 0)
{
contacts = supportContacts;
}
else
{
//No contacts provide support!
//Fall back to the ray cast result.
if (SupportRayData != null)
{
supportData = new SupportData
{
Position = SupportRayData.Value.HitData.Location,
Normal = SupportRayData.Value.HitData.Normal,
Depth = Vector3Ex.Dot(down, SupportRayData.Value.HitData.Normal) * (BottomDistance - SupportRayData.Value.HitData.T),
SupportObject = SupportRayData.Value.HitObject
};
}
else
{
supportData = new SupportData();
}
return;
}
//Compute a representative support from the set of contacts.
supportData.Position = contacts.Elements[0].Contact.Position;
supportData.Normal = contacts.Elements[0].Contact.Normal;
for (int i = 1; i < contacts.Count; i++)
{
Vector3Ex.Add(ref supportData.Position, ref contacts.Elements[i].Contact.Position, out supportData.Position);
Vector3Ex.Add(ref supportData.Normal, ref contacts.Elements[i].Contact.Normal, out supportData.Normal);
}
if (contacts.Count > 1)
{
Vector3Ex.Divide(ref supportData.Position, contacts.Count, out supportData.Position);
float length = supportData.Normal.LengthSquared();
if (length < Toolbox.Epsilon)
{
//It's possible that the normals have cancelled each other out- that would be bad!
//Just use an arbitrary support's normal in that case.
supportData.Normal = contacts.Elements[0].Contact.Normal;
}
else
{
Vector3Ex.Multiply(ref supportData.Normal, 1 / (float)Math.Sqrt(length), out supportData.Normal);
}
}
//Now that we have the normal, cycle through all the contacts again and find the deepest projected depth.
//Use that object as our support too.
float depth = -float.MaxValue;
Collidable supportObject = null;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
Vector3Ex.Dot(ref contacts.Elements[i].Contact.Normal, ref supportData.Normal, out dot);
dot = dot * contacts.Elements[i].Contact.PenetrationDepth;
if (dot > depth)
{
depth = dot;
supportObject = contacts.Elements[i].Collidable;
}
}
supportData.Depth = depth;
supportData.SupportObject = supportObject;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
System.Numerics.Vector3 axisA, axisB;
QuaternionEx.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
QuaternionEx.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
System.Numerics.Vector3 twistMeasureAxisA, twistMeasureAxisB;
QuaternionEx.Transform(ref LocalMeasurementAxisA, ref ConnectionA.Orientation, out twistMeasureAxisA);
QuaternionEx.Transform(ref LocalMeasurementAxisB, ref ConnectionB.Orientation, out twistMeasureAxisB);
//Compute the shortest rotation to bring axisB into alignment with axisA.
System.Numerics.Quaternion alignmentRotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref axisB, ref axisA, out alignmentRotation);
//Transform the measurement axis on B by the alignment quaternion.
QuaternionEx.Transform(ref twistMeasureAxisB, ref alignmentRotation, out twistMeasureAxisB);
//We can now compare the angle between the twist axes.
float error;
Vector3Ex.Dot(ref twistMeasureAxisA, ref twistMeasureAxisB, out error);
error = (float)Math.Acos(MathHelper.Clamp(error, -1, 1));
System.Numerics.Vector3 cross;
Vector3Ex.Cross(ref twistMeasureAxisA, ref twistMeasureAxisB, out cross);
float dot;
Vector3Ex.Dot(ref cross, ref axisA, out dot);
if (dot < 0)
{
error = -error;
}
//Compute the bias based upon the error.
velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * error, 0, 0);
//We can't just use the axes directly as jacobians. Consider 'cranking' one object around the other.
System.Numerics.Vector3 jacobian;
Vector3Ex.Add(ref axisA, ref axisB, out jacobian);
float lengthSquared = jacobian.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref jacobian, (float)Math.Sqrt(lengthSquared), out jacobian);
}
else
{
//The constraint is in an invalid configuration. Just ignore it.
jacobian = new System.Numerics.Vector3();
}
angularJacobianA = new Matrix3x3 {
M11 = jacobian.X, M12 = jacobian.Y, M13 = jacobian.Z
};
angularJacobianB = new Matrix3x3 {
M11 = -jacobian.X, M12 = -jacobian.Y, M13 = -jacobian.Z
};
}
