/// <summary>
/// Finds the intersection between the given line and the given plane.
/// </summary>
/// <param name="a">First endpoint of segment defining the line.</param>
/// <param name="b">Second endpoint of segment defining the line.</param>
/// <param name="p">Plane for comparison.</param>
/// <param name="t">Interval along line to intersection (A + t * AB).</param>
/// <param name="q">Intersection point.</param>
/// <returns>Whether or not the line intersects the plane. If false, the line is parallel to the plane's surface.</returns>
public static bool GetLinePlaneIntersection(ref Vector3 a, ref Vector3 b, ref Plane p, out float t, out Vector3 q)
{
Vector3 ab;
Vector3.Subtract(ref b, ref a, out ab);
float denominator;
Vector3.Dot(ref p.Normal, ref ab, out denominator);
if (denominator < Epsilon && denominator > -Epsilon)
{
//Surface of plane and line are parallel (or very close to it).
q = new Vector3();
t = float.MaxValue;
return false;
}
float numerator;
Vector3.Dot(ref p.Normal, ref a, out numerator);
t = (p.D - numerator) / denominator;
//Compute the intersection position.
Vector3.Multiply(ref ab, t, out q);
Vector3.Add(ref a, ref q, out q);
return true;
}
/// <summary>
/// Finds the intersection between the given ray and the given plane.
/// </summary>
/// <param name="ray">Ray to test against the plane.</param>
/// <param name="p">Plane for comparison.</param>
/// <param name="t">Interval along line to intersection (A + t * AB).</param>
/// <param name="q">Intersection point.</param>
/// <returns>Whether or not the line intersects the plane. If false, the line is parallel to the plane's surface.</returns>
public static bool GetRayPlaneIntersection(ref Ray ray, ref Plane p, out float t, out Vector3 q)
{
float denominator;
Vector3.Dot(ref p.Normal, ref ray.Direction, out denominator);
if (denominator < Epsilon && denominator > -Epsilon)
{
//Surface of plane and line are parallel (or very close to it).
q = new Vector3();
t = float.MaxValue;
return false;
}
float numerator;
Vector3.Dot(ref p.Normal, ref ray.Position, out numerator);
t = (p.D - numerator) / denominator;
//Compute the intersection position.
Vector3.Multiply(ref ray.Direction, t, out q);
Vector3.Add(ref ray.Position, ref q, out q);
return t >= 0;
}
bool TryToStepUsingContact(ref ContactData contact, ref Vector3 down, out Vector3 newPosition)
{
Vector3 position = characterBody.Position;
//The normal of the contact may not be facing perfectly out to the side.
//The detection process allows a bit of slop.
//Correct it by removing any component of the normal along the local up vector.
Vector3 normal = contact.Normal;
float dot;
Vector3.Dot(ref normal, ref down, out dot);
Vector3 error;
Vector3.Multiply(ref down, dot, out error);
Vector3.Subtract(ref normal, ref error, out normal);
normal.Normalize();
//Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions.
//Compute the ray origin location. Fire it out of the top of the character; if we're stepping, this must be a valid location.
//Putting it as high as possible helps to reject more invalid step geometry.
Ray ray;
float downRayLength = characterBody.Height;// MaximumStepHeight + upStepMargin;
Vector3.Multiply(ref down, characterBody.Height * .5f - downRayLength, out ray.Position);
Vector3.Add(ref ray.Position, ref position, out ray.Position);
ray.Direction = normal;
//Include a little margin in the length.
//Technically, the character only needs to teleport horizontally by the complicated commented expression.
//That puts it just far enough to have traction on the new surface.
//In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit,
//which can cause side effects for the character.
float horizontalOffsetAmount = characterBody.CollisionInformation.Shape.CollisionMargin;// (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0);
float length = characterBody.Radius + horizontalOffsetAmount;// -contact.PenetrationDepth;
if (QueryManager.RayCastHitAnything(ray, length))
{
//The step is obstructed!
newPosition = new Vector3();
return false;
}
//The down-cast ray origin has been verified by the previous ray cast.
//Let's look for a support!
Vector3 horizontalOffset;
Vector3.Multiply(ref normal, length, out horizontalOffset);
Vector3.Add(ref ray.Position, ref horizontalOffset, out ray.Position);
ray.Direction = down;
//Find the earliest hit, if any.
RayHit earliestHit;
if (!QueryManager.RayCast(ray, downRayLength, out earliestHit) || //Can't do anything if it didn't hit.
earliestHit.T <= 0 || //Can't do anything if the hit was invalid.
earliestHit.T - downRayLength > -minimumUpStepHeight || //Don't bother doing anything if the step is too small.
earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin) //Can't do anything if the step is too tall.
{
//No valid hit was detected.
newPosition = new Vector3();
return false;
}
//Ensure the candidate surface supports traction.
Vector3 supportNormal;
Vector3.Normalize(ref earliestHit.Normal, out supportNormal);
//Calibrate the normal to face in the same direction as the down vector for consistency.
Vector3.Dot(ref supportNormal, ref down, out dot);
if (dot < 0)
{
Vector3.Negate(ref supportNormal, out supportNormal);
dot = -dot;
}
//If the new surface does not have traction, do not attempt to step up.
if (dot < ContactCategorizer.TractionThreshold)
{
newPosition = new Vector3();
return false;
}
//Since contact queries are frequently expensive compared to ray cast tests,
//do one more ray cast test. This time, starting from the same position, cast upwards.
//In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray.
Vector3.Negate(ref down, out ray.Direction);
//Find the earliest hit, if any.
//RayHit earliestHitUp = new RayHit();
//earliestHitUp.T = float.MaxValue;
float upLength = characterBody.Height - earliestHit.T;
//If the sum of the up and down distances is less than the height, the character can't fit.
if (QueryManager.RayCastHitAnything(ray, upLength))
{
newPosition = new Vector3();
return false;
}
//By now, a valid ray hit has been found. Now we need to validate it using contact queries.
//This process is very similar in concept to the down step verification, but it has some extra
//requirements.
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//The transform must be modified to position the query body at the right location.
//The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support.
Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
Vector3 verticalOffset;
Vector3.Multiply(ref down, -downRayLength, out verticalOffset);
Vector3.Add(ref transform.Position, ref verticalOffset, out transform.Position);
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray downRay;
characterBody.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position);
downRay.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot);
Plane plane = new Plane(supportNormal, dot);
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = -maximumStepHeight;
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var supportContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var sideContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var headContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection))
{
hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration;
if (hitT < highestBound)
{
//Don't try a location known to be too high.
hitT = highestBound;
}
currentOffset = hitT;
if (currentOffset > lowestBound)
lowestBound = currentOffset;
candidatePosition = characterBody.Position + down * currentOffset + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return false;
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
} //TODO: If the ray cast doesn't hit, that could be used to early out... Then again, it pretty much can't happen.
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return false;
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
//Couldn't find a candidate.
newPosition = new Vector3();
return false;
}
/// <summary>
/// Finds the intersection between the given segment and the given plane.
/// </summary>
/// <param name="a">First endpoint of segment.</param>
/// <param name="b">Second endpoint of segment.</param>
/// <param name="p">Plane for comparison.</param>
/// <param name="t">Interval along segment to intersection.</param>
/// <param name="q">Intersection point.</param>
/// <returns>Whether or not the segment intersects the plane.</returns>
public static bool GetSegmentPlaneIntersection(Vector3 a, Vector3 b, Plane p, out float t, out Vector3 q)
{
return GetLinePlaneIntersection(ref a, ref b, ref p, out t, out q) && t >= 0 && t <= 1;
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
//Collect references, pick the mode, and configure the coefficients to be used by the solver.
Vector3 movementDirectionIn3d;
GetMovementDirectionIn3D(out movementDirectionIn3d);
bool isTryingToMove = movementDirectionIn3d.LengthSquared() > 0;
if (supportData.SupportObject != null)
{
if (supportData.HasTraction)
{
MovementMode = MovementMode.Traction;
maxSpeed = speed;
maxForce = maximumForce;
}
else
{
MovementMode = MovementMode.Sliding;
maxSpeed = slidingSpeed;
maxForce = maximumSlidingForce;
}
}
else
{
MovementMode = MovementMode.Floating;
maxSpeed = airSpeed;
maxForce = maximumAirForce;
supportEntity = null;
}
if (!isTryingToMove)
maxSpeed = 0;
maxSpeed *= SpeedScale;
maxForce *= dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = character.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3 lineStart = movementDirectionIn3d;
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
Vector3 supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3();
angularJacobianB2 = new Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirectionIn3d;
linearJacobianA2 = Vector3.Cross(linearJacobianA1, character.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = maxSpeed;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3 intermediate;
inverseMass = character.Body.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2x2.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(character.Body.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 && supportData.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 && supportData.HasTraction && supportEntity != null)
{
//Compute the time it usually takes for the character to slow down while it has traction.
var tractionDecelerationTime = speed / (maximumForce * character.Body.InverseMass);
if (timeSinceTransition >= 0 && timeSinceTransition < tractionDecelerationTime)
timeSinceTransition += dt;
if (timeSinceTransition >= tractionDecelerationTime)
{
Vector3.Multiply(ref downDirection, character.Body.Radius, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + character.Body.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, character.Body.Radius, out targetPosition);
targetPosition += character.Body.Position;
Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3 error;
Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > .15f * .15f)
{
Vector3.Multiply(ref downDirection, character.Body.Radius, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + character.Body.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportData.HasTraction;
previousSupportEntity = supportEntity;
}
/// <summary>
/// Determines if a down step is possible, and if so, computes the location to which the character should teleport.
/// </summary>
/// <param name="newPosition">New position the character should teleport to if the down step is accepted.</param>
/// <returns>Whether or not the character should attempt to step down.</returns>
public bool TryToStepDown(out Vector3 newPosition)
{
//Don't bother trying to step down if we already have a support contact or if the support ray doesn't have traction.
if (!(SupportFinder.Supports.Count == 0 && SupportFinder.SupportRayData != null && SupportFinder.SupportRayData.Value.HasTraction))
{
newPosition = new Vector3();
return false;
}
if (!(SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.BottomDistance > minimumDownStepHeight)) //Don't do expensive stuff if it's, at most, a super tiny step that gravity will take care of.
{
newPosition = new Vector3();
return false;
}
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
Vector3 normal = SupportFinder.SupportRayData.Value.HitData.Normal;
Vector3 down = characterBody.orientationMatrix.Down;
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray ray;
characterBody.CollisionInformation.Shape.GetExtremePoint(normal, ref transform, out ray.Position);
ray.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Plane plane = new Plane(normal, Vector3.Dot(SupportFinder.SupportRayData.Value.HitData.Location, normal));
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = 0;
//The lowest possible distance is the ray distance plus the collision margin because the ray could theoretically be on the outskirts of the collision margin
//where the shape would actually have to move more than the bottom distance difference would imply.
//(Could compute the true lowest bound analytically based on the horizontal position of the ray...)
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin + SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.BottomDistance;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var supportContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var sideContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var headContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref ray, ref plane, out hitT, out intersection))
{
currentOffset = hitT + CollisionDetectionSettings.AllowedPenetration * 0.5f;
candidatePosition = characterBody.Position + down * currentOffset;
switch (TryDownStepPosition(ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > minimumDownStepHeight && currentOffset < maximumStepHeight)
{
newPosition = characterBody.Position + currentOffset * down;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down;
switch (TryDownStepPosition(ref candidatePosition, ref down, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts, out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > minimumDownStepHeight && currentOffset < maximumStepHeight)
{
newPosition = characterBody.Position + currentOffset * down;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Couldn't find a candidate.
newPosition = new Vector3();
return false;
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}
/// <summary>
/// Finds the intersection between the given segment and the given plane.
/// </summary>
/// <param name="a">First endpoint of segment.</param>
/// <param name="b">Second enpoint of segment.</param>
/// <param name="p">Plane for comparison.</param>
/// <param name="q">Intersection point.</param>
/// <returns>Whether or not the segment intersects the plane.</returns>
public static bool GetSegmentPlaneIntersection(System.Numerics.Vector3 a, System.Numerics.Vector3 b, Plane p, out System.Numerics.Vector3 q)
{
float t;
return GetLinePlaneIntersection(ref a, ref b, ref p, out t, out q) && t >= 0 && t <= 1;
}
private void BuildSimulation(Vector3 offset, Simulator simulator)
{
//Create a lattice of dynamic objects.
int width = 30;
int height = 10;
int length = 30;
float spacing = 3;
var dynamics = new LinearDynamic[width, height, length];
for (int widthIndex = 0; widthIndex < width; ++widthIndex)
{
for (int heightIndex = 0; heightIndex < height; ++heightIndex)
{
for (int lengthIndex = 0; lengthIndex < length; ++lengthIndex)
{
var dynamic = new LinearDynamic(10)
{
Position = offset + new Vector3(spacing * widthIndex, spacing * heightIndex + 10, spacing * lengthIndex)
};
dynamics[widthIndex, heightIndex, lengthIndex] = dynamic;
simulator.Add(dynamic);
}
}
}
Vector3 normal = new Vector3(0, 1, 0);
Plane plane = new Plane(normal, -Vector3.Dot(offset, normal));
//Create a bunch of connections between the dynamic objects.
for (int widthIndex = 0; widthIndex < width; ++widthIndex)
{
for (int heightIndex = 0; heightIndex < height; ++heightIndex)
{
for (int lengthIndex = 0; lengthIndex < length; ++lengthIndex)
{
//Create a connection with the dynamics at +x, +y, +z, +x+y+z, -x+y+z, +x-y+z, +x+y-z
//+x
if (widthIndex + 1 < width)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex, lengthIndex]));
}
//+y
if (heightIndex + 1 < height)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex, heightIndex + 1, lengthIndex]));
}
//+z
if (lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex, heightIndex, lengthIndex + 1]));
}
//+x+y+z
if (widthIndex + 1 < width && heightIndex + 1 < height && lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex + 1, lengthIndex + 1]));
}
//-x+y+z
if (widthIndex - 1 >= 0 && heightIndex + 1 < height && lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex - 1, heightIndex + 1, lengthIndex + 1]));
}
//+x-y+z
if (widthIndex + 1 < width && heightIndex - 1 >= 0 && lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex - 1, lengthIndex + 1]));
}
//+x+y-z
if (widthIndex + 1 < width && heightIndex + 1 < height && lengthIndex - 1 >= 0)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex + 1, lengthIndex - 1]));
}
//Create a plane constraint to stop the object from falling.
simulator.Add(new PlaneCollisionConstraint(dynamics[widthIndex, heightIndex, lengthIndex], plane));
}
}
}
}
/// <summary>
/// Determines if and when the ray intersects the plane.
/// </summary>
/// <param name="plane">Plane to test against.</param>
/// <param name="t">The length along the ray to the impact, if any impact occurs.</param>
/// <returns>True if the ray intersects the target, false otherwise.</returns>
public bool Intersects(Plane plane, out float t)
{
return Intersects(ref plane, out t);
}
/// <summary>
/// Determines if and when the ray intersects the plane.
/// </summary>
/// <param name="plane">Plane to test against.</param>
/// <param name="t">The length along the ray to the impact, if any impact occurs.</param>
/// <returns>True if the ray intersects the target, false otherwise.</returns>
public bool Intersects(ref Plane plane, out float t)
{
float velocity;
Vector3.Dot(ref Direction, ref plane.Normal, out velocity);
if (Math.Abs(velocity) < Toolbox.Epsilon)
{
t = 0;
return false;
}
float distanceAlongNormal;
Vector3.Dot(ref Position, ref plane.Normal, out distanceAlongNormal);
distanceAlongNormal += plane.D;
t = -distanceAlongNormal / velocity;
return t >= -Toolbox.Epsilon;
}
public PlaneCollisionConstraint(LinearDynamic dynamic, Plane plane)
{
Dynamic = dynamic;
++dynamic.ConstraintCount;
this.plane = plane;
}