/// <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.
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
Quaternion.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 Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3();
}
//Pull the hit into world space.
Quaternion.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.
Vector2 planeDirection = new 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);
}
Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
Vector2 closestPoint;
Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2.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.
Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3.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 Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3();
}
//Pull the hit into world space.
Quaternion.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;
Vector3 planeIntersection;
Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3.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.
Quaternion.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;
Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3.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.
Quaternion.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);
}
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>
/// 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>
/// Gets the intersection between the box and the ray.
/// </summary>
/// <param name="ray">Ray to test against the box.</param>
/// <param name="transform">Transform of the shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
/// <param name="hit">Hit data for the raycast, 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)
{
hit = new RayHit();
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Vector3 localOrigin;
Vector3.Subtract(ref ray.Position, ref transform.Position, out localOrigin);
Quaternion.Transform(ref localOrigin, ref conjugate, out localOrigin);
Vector3 localDirection;
Quaternion.Transform(ref ray.Direction, ref conjugate, out localDirection);
Vector3 normal = Toolbox.ZeroVector;
float temp, tmin = 0, tmax = maximumLength;
if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth))
{
return(false);
}
float inverseDirection = 1 / localDirection.X;
float t1 = (-halfWidth - localOrigin.X) * inverseDirection;
float t2 = (halfWidth - localOrigin.X) * inverseDirection;
var tempNormal = new Vector3(-1, 0, 0);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
{
normal = tempNormal;
}
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight))
{
return(false);
}
inverseDirection = 1 / localDirection.Y;
t1 = (-halfHeight - localOrigin.Y) * inverseDirection;
t2 = (halfHeight - localOrigin.Y) * inverseDirection;
tempNormal = new Vector3(0, -1, 0);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
{
normal = tempNormal;
}
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength))
{
return(false);
}
inverseDirection = 1 / localDirection.Z;
t1 = (-halfLength - localOrigin.Z) * inverseDirection;
t2 = (halfLength - localOrigin.Z) * inverseDirection;
tempNormal = new Vector3(0, 0, -1);
if (t1 > t2)
{
temp = t1;
t1 = t2;
t2 = temp;
tempNormal *= -1;
}
temp = tmin;
tmin = Math.Max(tmin, t1);
if (temp != tmin)
{
normal = tempNormal;
}
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
hit.T = tmin;
Vector3.Multiply(ref ray.Direction, tmin, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Quaternion.Transform(ref normal, ref transform.Orientation, out normal);
hit.Normal = normal;
return(true);
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
///<param name="weight">Weight of the entry. This defines how much the entry contributes to its owner
/// for the purposes of center of rotation computation.</param>
public CompoundShapeEntry(EntityShape shape, float weight)
{
LocalTransform = RigidTransform.Identity;
Shape = shape;
Weight = weight;
}
///<summary>
/// Constructs a new compound shape entry using the volume of the shape as a weight.
///</summary>
///<param name="shape">Shape to use.</param>
public CompoundShapeEntry(EntityShape shape)
{
LocalTransform = RigidTransform.Identity;
Shape = shape;
Weight = shape.Volume;
}
