///<summary>
/// Gets the extreme point of the shape in local space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint)
{
Vector3 d;
Matrix3x3.TransformTranspose(ref direction, ref transform, out d);
shape.GetLocalExtremePoint(d, out extremePoint);
Matrix3x3.Transform(ref extremePoint, ref transform, out extremePoint);
}
/// <summary>
/// Gets the bounding box of the convex shape transformed first into world space, and then into the local space of another affine transform.
/// </summary>
/// <param name="shapeTransform">Transform to use to put the shape into world space.</param>
/// <param name="spaceTransform">Used as the frame of reference to compute the bounding box.
/// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param>
/// <param name="sweep">Vector to expand the bounding box with in local space.</param>
/// <param name="boundingBox">Bounding box in the local space.</param>
public void GetSweptLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, ref Vector3 sweep, out BoundingBox boundingBox)
{
GetLocalBoundingBox(ref shapeTransform, ref spaceTransform, out boundingBox);
Vector3 expansion;
Matrix3x3.TransformTranspose(ref sweep, ref spaceTransform.LinearTransform, out expansion);
Toolbox.ExpandBoundingBox(ref boundingBox, ref expansion);
}
//Expand the convex's bounding box to include the mobile mesh's movement.
protected internal override int FindOverlappingTriangles(float dt)
{
BoundingBox boundingBox;
AffineTransform transform = new AffineTransform(mesh.worldTransform.Orientation, mesh.worldTransform.Position);
convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref transform, out boundingBox);
Vector3 transformedVelocity;
//Compute the relative velocity with respect to the mesh. The mesh's bounding tree is NOT expanded with velocity,
//so whatever motion there is between the two objects needs to be included in the convex's bounding box.
if (convex.entity != null)
{
transformedVelocity = convex.entity.linearVelocity;
}
else
{
transformedVelocity = new Vector3();
}
if (mesh.entity != null)
{
Vector3.Subtract(ref transformedVelocity, ref mesh.entity.linearVelocity, out transformedVelocity);
}
//The linear transform is known to be orientation only, so using the transpose is allowed.
Matrix3x3.TransformTranspose(ref transformedVelocity, ref transform.LinearTransform, out transformedVelocity);
Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity);
if (transformedVelocity.X > 0)
{
boundingBox.Max.X += transformedVelocity.X;
}
else
{
boundingBox.Min.X += transformedVelocity.X;
}
if (transformedVelocity.Y > 0)
{
boundingBox.Max.Y += transformedVelocity.Y;
}
else
{
boundingBox.Min.Y += transformedVelocity.Y;
}
if (transformedVelocity.Z > 0)
{
boundingBox.Max.Z += transformedVelocity.Z;
}
else
{
boundingBox.Min.Z += transformedVelocity.Z;
}
mesh.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, overlappedTriangles);
return(overlappedTriangles.Count);
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.</param>
public void SetWorldAxes(Matrix3x3 matrix)
{
if (Math.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint.");
}
primaryAxis = Vector3.Normalize(matrix.Backward);
xAxis = Vector3.Normalize(matrix.Right);
Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis);
Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis);
}
///<summary>
/// Updates the cached simplex with the latest run's results.
///</summary>
///<param name="simplex">Simplex to update.</param>
public void UpdateCachedSimplex(ref CachedSimplex simplex)
{
simplex.LocalSimplexA = SimplexA;
switch (State)
{
case SimplexState.Point:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Quaternion conjugate;
Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
Quaternion.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
break;
case SimplexState.Segment:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
break;
case SimplexState.Triangle:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
break;
case SimplexState.Tetrahedron:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D);
break;
}
simplex.State = State;
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.
/// The matrix's up vector is used as the y axis.</param>
public void SetWorldAxes(Matrix3x3 matrix)
{
if (Math.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon ||
Math.Abs(Vector3.Dot(matrix.Backward, matrix.Up)) > Toolbox.BigEpsilon ||
Math.Abs(Vector3.Dot(matrix.Right, matrix.Up)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two.");
}
primaryAxis = Vector3.Normalize(matrix.Backward);
xAxis = Vector3.Normalize(matrix.Right);
yAxis = Vector3.Normalize(matrix.Up);
Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis);
Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis);
Matrix3x3.TransformTranspose(ref this.yAxis, ref rotationMatrix, out localYAxis);
}
///<summary>
/// Tests a ray against the instance.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref worldTransform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.TransformTranspose(ref rayHit.Normal, ref inverse.LinearTransform, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
///<summary>
/// Tests a ray against the surface of the mesh. This does not take into account solidity.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
/// <summary>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//Find all hits. Use the count to determine the ray started inside or outside.
//If it starts inside and we're in 'solid' mode, then return the ray start.
//The raycast must be of infinite length at first. This allows it to determine
//if it is inside or outside.
if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit))
{
//It was inside!
rayHit = new RayHit()
{
Location = ray.Position, Normal = Vector3.Zero, T = 0
};
return(true);
}
else
{
if (rayHit.T < maximumLength)
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
}
else
{
//The hit was too far away, or there was no hit (in which case T would be float.MaxValue).
return(false);
}
return(true);
}
}
else
{
//Just do a normal raycast since the object isn't solid.
TriangleSidedness sidedness;
switch (Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
default:
sidedness = TriangleSidedness.DoubleSided;
break;
}
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
}
rayHit = new RayHit();
return(false);
}
///<summary>
/// Tests a ray against the terrain shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="transform">Transform to apply to the terrain shape during the test.</param>
///<param name="sidedness">Sidedness of the triangles to use when raycasting.</param>
///<param name="hit">Hit data of the ray cast, if any.</param>
///<returns>Whether or not the ray hit the transformed terrain shape.</returns>
public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, TriangleSidedness sidedness, out RayHit hit)
{
hit = new RayHit();
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref transform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
//Use rasterizey traversal.
//The origin is at 0,0,0 and the map goes +X, +Y, +Z.
//if it's before the origin and facing away, or outside the max and facing out, early out.
float maxX = heights.GetLength(0) - 1;
float maxZ = heights.GetLength(1) - 1;
Vector3 progressingOrigin = localRay.Position;
float distance = 0;
//Check the outside cases first.
if (progressingOrigin.X < 0)
{
if (localRay.Direction.X > 0)
{
//Off the left side.
float timeToMinX = -progressingOrigin.X / localRay.Direction.X;
distance += timeToMinX;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false); //Outside and pointing away from the terrain.
}
}
else if (progressingOrigin.X > maxX)
{
if (localRay.Direction.X < 0)
{
//Off the left side.
float timeToMinX = -(progressingOrigin.X - maxX) / localRay.Direction.X;
distance += timeToMinX;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false); //Outside and pointing away from the terrain.
}
}
if (progressingOrigin.Z < 0)
{
if (localRay.Direction.Z > 0)
{
float timeToMinZ = -progressingOrigin.Z / localRay.Direction.Z;
distance += timeToMinZ;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false);
}
}
else if (progressingOrigin.Z > maxZ)
{
if (localRay.Direction.Z < 0)
{
float timeToMinZ = -(progressingOrigin.Z - maxZ) / localRay.Direction.Z;
distance += timeToMinZ;
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false);
}
}
if (distance > maximumLength)
{
return(false);
}
//By now, we should be entering the main body of the terrain.
int xCell = (int)progressingOrigin.X;
int zCell = (int)progressingOrigin.Z;
//If it's hitting the border and going in, then correct the index
//so that it will initially target a valid quad.
//Without this, a quad beyond the border would be tried and failed.
if (xCell == heights.GetLength(0) - 1 && localRay.Direction.X < 0)
{
xCell = heights.GetLength(0) - 2;
}
if (zCell == heights.GetLength(1) - 1 && localRay.Direction.Z < 0)
{
zCell = heights.GetLength(1) - 2;
}
while (true)
{
//Check for a miss.
if (xCell < 0 ||
zCell < 0 ||
xCell >= heights.GetLength(0) - 1 ||
zCell >= heights.GetLength(1) - 1)
{
return(false);
}
//Test the triangles of this cell.
Vector3 v1, v2, v3, v4;
// v3 v4
// v1 v2
GetLocalPosition(xCell, zCell, out v1);
GetLocalPosition(xCell + 1, zCell, out v2);
GetLocalPosition(xCell, zCell + 1, out v3);
GetLocalPosition(xCell + 1, zCell + 1, out v4);
RayHit hit1, hit2;
bool didHit1;
bool didHit2;
//Don't bother doing ray intersection tests if the ray can't intersect it.
float highest = v1.Y;
float lowest = v1.Y;
if (v2.Y > highest)
{
highest = v2.Y;
}
else if (v2.Y < lowest)
{
lowest = v2.Y;
}
if (v3.Y > highest)
{
highest = v3.Y;
}
else if (v3.Y < lowest)
{
lowest = v3.Y;
}
if (v4.Y > highest)
{
highest = v4.Y;
}
else if (v4.Y < lowest)
{
lowest = v4.Y;
}
if (!(progressingOrigin.Y > highest && localRay.Direction.Y > 0 ||
progressingOrigin.Y < lowest && localRay.Direction.Y < 0))
{
if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
//Always perform the raycast as if Y+ in local space is the way the triangles are facing.
didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit1);
didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v2, ref v4, ref v3, out hit2);
}
else //if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomRightUpperLeft)
{
didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v4, out hit1);
didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v4, ref v3, out hit2);
}
if (didHit1 && didHit2)
{
if (hit1.T < hit2.T)
{
Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit1.T;
return(true);
}
Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit2.T;
return(true);
}
else if (didHit1)
{
Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit1.T;
return(true);
}
else if (didHit2)
{
Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit2.T;
return(true);
}
}
//Move to the next cell.
float timeToX;
if (localRay.Direction.X < 0)
{
timeToX = -(progressingOrigin.X - xCell) / localRay.Direction.X;
}
else if (localRay.Direction.X > 0)
{
timeToX = (xCell + 1 - progressingOrigin.X) / localRay.Direction.X;
}
else
{
timeToX = float.MaxValue;
}
float timeToZ;
if (localRay.Direction.Z < 0)
{
timeToZ = -(progressingOrigin.Z - zCell) / localRay.Direction.Z;
}
else if (localRay.Direction.Z > 0)
{
timeToZ = (zCell + 1 - progressingOrigin.Z) / localRay.Direction.Z;
}
else
{
timeToZ = float.MaxValue;
}
//Move to the next cell.
if (timeToX < timeToZ)
{
if (localRay.Direction.X < 0)
{
xCell--;
}
else
{
xCell++;
}
distance += timeToX;
if (distance > maximumLength)
{
return(false);
}
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToX, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
if (localRay.Direction.Z < 0)
{
zCell--;
}
else
{
zCell++;
}
distance += timeToZ;
if (distance > maximumLength)
{
return(false);
}
Vector3 increment;
Vector3.Multiply(ref localRay.Direction, timeToZ, out increment);
Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
}
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
isTryingToMove = movementDirection3d.LengthSquared() > 0;
maxForceDt = MaximumForce * dt;
maxAccelerationForceDt = MaximumAccelerationForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3 lineStart = movementDirection3d;
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
Vector3 supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3();
angularJacobianB2 = new Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirection3d;
linearJacobianA2 = Vector3.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = isTryingToMove ? TargetSpeed : 0;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3 intermediate;
inverseMass = characterBody.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2x2.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(characterBody.Mass, out massMatrix);
}
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
{
var distanceToBottomOfCharacter = supportFinder.BottomDistance;
if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
{
timeSinceTransition += dt;
}
if (timeSinceTransition >= timeUntilPositionAnchor)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3 error;
Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
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;
Vector3.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.
Vector3.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 = Vector3.Up;
rayDirectionLength = 1;
}
}
Vector3.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);
Vector3.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position);
newContact.Normal = hit.Normal;
newContact.Normal.Normalize();
float factor;
Vector3.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 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;
}
}
}
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Compute the local triangle vertices.
//TODO: this could be quicker and cleaner.
localTriangleShape.collisionMargin = triangle.Shape.collisionMargin;
localTriangleShape.sidedness = triangle.Shape.sidedness;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation);
Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC);
Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA);
Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB);
Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC);
Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);
//Now, generate a contact between the two shapes.
ContactData contact;
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList))
{
for (int i = 0; i < contactList.Count; i++)
{
contactList.Get(i, out contact);
//Put the contact into world space.
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check if the contact is unique before proceeding.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
Add(ref contact);
}
}
}
}
else
{
//Clear out the contacts, it's separated.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
