void ComputeSolidSidedness()
{
//Raycast against the mesh.
//If there's an even number of hits, then the ray start point is outside.
//If there's an odd number of hits, then the ray start point is inside.
//If the start is outside, then take the earliest toi hit and calibrate sidedness based on it.
//If the start is inside, then take the latest toi hit and calibrate sidedness based on it.
//This test assumes consistent winding across the entire mesh as well as a closed surface.
//If those assumptions are not correct, then the raycast cannot determine inclusion or exclusion,
//or there exists no calibration that will work across the entire surface.
//Pick a ray direction that goes to a random location on the mesh.
//A vertex would work, but targeting the middle of a triangle avoids some edge cases.
var ray = new Ray();
Vector3 vA, vB, vC;
triangleMesh.Data.GetTriangle(((triangleMesh.Data.indices.Length / 3) / 2) * 3, out vA, out vB, out vC);
ray.Direction = (vA + vB + vC) / F64.C3;
ray.Direction.Normalize();
SidednessWhenSolid = ComputeSolidSidednessHelper(ray);
//ComputeSolidSidednessHelper is separated into another function just in case multiple queries were desired for validation.
//If multiple rays returned different sidednesses, the shape would be inconsistent.
}
/// <summary>
/// Creates and instance of InstancedTriangleMesh
/// Call the Static Method GetInstacedMesh to get the InstancedMeshShape obj
/// </summary>
/// <param name="InstancedMeshShape">The instanced mesh shape.</param>
/// <param name="pos">The pos.</param>
/// <param name="rotation">The rotation.</param>
/// <param name="scale">The scale.</param>
/// <param name="materialDescription">The material description.</param>
/// <param name="TriangleSidedness">The triangle sidedness.</param>
public InstancedTriangleMeshObject(InstancedMeshShape InstancedMeshShape, Vector3 pos, Matrix rotation, Vector3 scale, MaterialDescription materialDescription, TriangleSidedness TriangleSidedness = TriangleSidedness.Counterclockwise)
{
instancedMesh = new InstancedMesh(InstancedMeshShape,new BEPUphysics.MathExtensions.AffineTransform(scale,Quaternion.CreateFromRotationMatrix(rotation),pos));
instancedMesh.Material = new BEPUphysics.Materials.Material(materialDescription.StaticFriction, materialDescription.DynamicFriction, materialDescription.Bounciness);
instancedMesh.Sidedness = TriangleSidedness;
this.rotation = rotation;
this.scale = scale;
}
///<summary>
/// Constructs a new mobile mesh shape from cached data.
///</summary>
///<param name="meshData">Mesh data reprsenting the shape. Should already be properly centered.</param>
/// <param name="hullVertices">Outer hull vertices of the mobile mesh shape used to quickly compute the bounding box.</param>
///<param name="solidity">Solidity state of the shape.</param>
/// <param name="sidednessWhenSolid">Triangle sidedness to use when the shape is solid.</param>
/// <param name="collisionMargin">Collision margin used to expand the mesh triangles.</param>
/// <param name="volumeDescription">Description of the volume and its distribution in the shape. Assumed to be correct; no processing or validation is performed.</param>
public MobileMeshShape(TransformableMeshData meshData, IList <Vector3> hullVertices, MobileMeshSolidity solidity, TriangleSidedness sidednessWhenSolid, Fix64 collisionMargin, EntityShapeVolumeDescription volumeDescription)
{
triangleMesh = new TriangleMesh(meshData);
this.hullVertices = new RawList <Vector3>(hullVertices);
meshCollisionMargin = collisionMargin;
this.solidity = solidity;
SidednessWhenSolid = sidednessWhenSolid;
UpdateEntityShapeVolume(volumeDescription);
}
void ComputeSolidSidedness()
{
//Raycast against the mesh.
//If there's an even number of hits, then the ray start point is outside.
//If there's an odd number of hits, then the ray start point is inside.
//If the start is outside, then take the earliest toi hit and calibrate sidedness based on it.
//If the start is inside, then take the latest toi hit and calibrate sidedness based on it.
//This test assumes consistent winding across the entire mesh as well as a closed surface.
//If those assumptions are not correct, then the raycast cannot determine inclusion or exclusion,
//or there exists no calibration that will work across the entire surface.
//Pick a ray direction that goes to a random location on the mesh.
//A vertex would work, but targeting the middle of a triangle avoids some edge cases.
var ray = new Ray();
Vector3 vA, vB, vC;
triangleMesh.Data.GetTriangle(((triangleMesh.Data.indices.Length / 3) / 2) * 3, out vA, out vB, out vC);
ray.Direction = (vA + vB + vC) / 3;
ray.Direction.Normalize();
solidSidedness = ComputeSolidSidednessHelper(ray);
//TODO: Positions need to be valid for the verifying directions to work properly.
////Find another direction and test it to corroborate the first test.
//Ray alternateRay;
//alternateRay.Position = ray.Position;
//Vector3.Cross(ref ray.Direction, ref Toolbox.UpVector, out alternateRay.Direction);
//float lengthSquared = alternateRay.Direction.LengthSquared();
//if (lengthSquared < Toolbox.Epsilon)
//{
// Vector3.Cross(ref ray.Direction, ref Toolbox.RightVector, out alternateRay.Direction);
// lengthSquared = alternateRay.Direction.LengthSquared();
//}
//Vector3.Divide(ref alternateRay.Direction, (float)Math.Sqrt(lengthSquared), out alternateRay.Direction);
//var sidednessCandidate2 = ComputeSolidSidednessHelper(alternateRay);
//if (sidednessCandidate == sidednessCandidate2)
//{
// //The two tests agreed! It's very likely that the sidedness is, in fact, in this direction.
// solidSidedness = sidednessCandidate;
//}
//else
//{
// //The two tests disagreed. Tiebreaker!
// Vector3.Cross(ref alternateRay.Direction, ref ray.Direction, out alternateRay.Direction);
// solidSidedness = ComputeSolidSidednessHelper(alternateRay);
//}
}
//TODO: Having a specialized triangle-triangle pair test would be nice. Even if it didn't use an actual triangle-triangle test, certain assumptions could still make it speedier and more elegant.
//"Closest points between triangles" + persistent manifolding would probably be the best approach (a lot faster than the triangle-convex general case anyway).
public override bool GenerateContactCandidates(TriangleShape triangle,
out TinyStructList <ContactData> contactList)
{
if (base.GenerateContactCandidates(triangle, out contactList))
{
//The triangle-convex pair test has already rejected contacts whose normals would violate the first triangle's sidedness.
//However, since it's a vanilla triangle-convex test, it doesn't know about the sidedness of the other triangle!
TriangleShape shape = (TriangleShape)convex;
Vector3 normal;
//Lots of recalculating ab-bc!
Vector3 ab, ac;
Vector3.Subtract(ref shape.vB, ref shape.vA, out ab);
Vector3.Subtract(ref shape.vC, ref shape.vA, out ac);
Vector3.Cross(ref ab, ref ac, out normal);
TriangleSidedness sidedness = shape.sidedness;
if (sidedness != TriangleSidedness.DoubleSided)
{
for (int i = contactList.Count - 1; i >= 0; i--)
{
ContactData item;
contactList.Get(i, out item);
float dot;
Vector3.Dot(ref item.Normal, ref normal, out dot);
if (sidedness == TriangleSidedness.Clockwise)
{
if (dot < 0)
{
contactList.RemoveAt(i);
}
}
else
{
if (dot > 0)
{
contactList.RemoveAt(i);
}
}
}
}
return(contactList.Count > 0);
}
return(false);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, IList <RayHit> hits)
{
var hitElements = Resources.GetIntList();
tree.GetOverlaps(ray, maximumLength, hitElements);
for (int i = 0; i < hitElements.Count; i++)
{
Vector3 v1, v2, v3;
data.GetTriangle(hitElements[i], out v1, out v2, out v3);
RayHit hit;
if (Toolbox.FindRayTriangleIntersection(ref ray, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit))
{
hits.Add(hit);
}
}
Resources.GiveBack(hitElements);
return(hits.Count > 0);
}
///<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);
}
} // Uninitialize
#endregion
#region Create Static Collidable From Model Filter
/// <summary>
/// Creates and assign a static mesh usign the model stored in the model filter component.
/// </summary>
/// <param name="triangleSidedness">A triangle can be double sided, or allow one of its sides to let interacting objects through.</param>
public void CreateStaticCollidableFromModelFilter(TriangleSidedness triangleSidedness = TriangleSidedness.Counterclockwise)
{
ModelFilter modelFilter = ((GameObject3D)Owner).ModelFilter;
if (modelFilter != null && modelFilter.Model != null && modelFilter.Model is FileModel)
{
Vector3[] vertices;
int[] indices;
TriangleMesh.GetVerticesAndIndicesFromModel(((FileModel)modelFilter.Model).Resource, out vertices, out indices);
StaticMesh staticMesh = new StaticMesh(vertices, indices)
{
Sidedness = triangleSidedness
};
StaticCollidable = staticMesh;
}
else
{
throw new InvalidOperationException("Static Collider: Model filter not present, model not present or model is not a FileModel.");
}
} // CreateStaticCollidableFromModelFilter
///<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 triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="rayHit">Hit data for the ray, 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)
{
var rayHits = Resources.GetRayHitList();
bool toReturn = RayCast(ray, maximumLength, sidedness, rayHits);
if (toReturn)
{
rayHit = rayHits[0];
for (int i = 1; i < rayHits.Count; i++)
{
RayHit hit = rayHits[i];
if (hit.T < rayHit.T)
{
rayHit = hit;
}
}
}
else
{
rayHit = new RayHit();
}
Resources.GiveBack(rayHits);
return(toReturn);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, TriangleSidedness sidedness, IList <RayHit> hits)
{
return(RayCast(ray, float.MaxValue, sidedness, hits));
}
///<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 mesh.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length to test in units of the ray direction's length.</param>
///<param name="sidedness">Sidedness to use when raycasting. Doesn't have to be the same as the mesh's own sidedness.</param>
///<param name="rayHit">Data about the ray's intersection with 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)
{
return(mesh.RayCast(ray, maximumLength, sidedness, out rayHit));
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//TODO: This conditional early outing stuff could be pulled up into a common system, along with most of the pair handler.
var overlap = BroadPhaseOverlap;
var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(mobileMesh.IsActive || (convex.entity == null ? false : convex.entity.activityInformation.IsActive)) && //At least one has to be active.
(
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous
)
)
)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convex.entity != null)
{
Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity);
}
else
{
Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
TriangleSidedness sidedness = mobileMesh.Shape.Sidedness;
Matrix3X3 orientation;
Matrix3X3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation);
var triangle = Resources.GetTriangle();
triangle.collisionMargin = 0;
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < MeshManifold.overlappedTriangles.count; i++)
{
MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data;
int triangleIndex = MeshManifold.overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
Matrix3X3.Transform(ref triangle.vA, ref orientation, out triangle.vA);
Matrix3X3.Transform(ref triangle.vB, ref orientation, out triangle.vB);
Matrix3X3.Transform(ref triangle.vC, ref orientation, out triangle.vC);
Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA);
Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB);
Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
if (sidedness != TriangleSidedness.DoubleSided)
{
Vector3 AB, AC;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
Resources.GiveBack(triangle);
}
}
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="rayHit">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, TriangleSidedness sidedness, out RayHit rayHit)
{
return RayCast(ray, float.MaxValue, sidedness, out rayHit);
}
/// <summary>
/// Creates and instance of InstancedTriangleMesh
/// Call the Static Method GetInstacedMesh to get the InstancedMeshShape obj
/// </summary>
/// <param name="InstancedMeshShape">The instanced mesh shape.</param>
/// <param name="pos">The pos.</param>
/// <param name="rotation">The rotation.</param>
/// <param name="scale">The scale.</param>
/// <param name="materialDescription">The material description.</param>
/// <param name="TriangleSidedness">The triangle sidedness.</param>
public InstancedTriangleMeshObject(InstancedMeshShape InstancedMeshShape, Vector3 pos, Matrix rotation, Vector3 scale, MaterialDescription materialDescription, TriangleSidedness TriangleSidedness = TriangleSidedness.Counterclockwise)
{
instancedMesh = new InstancedMesh(InstancedMeshShape, new BEPUphysics.MathExtensions.AffineTransform(scale, Quaternion.CreateFromRotationMatrix(rotation), pos));
instancedMesh.Material = new BEPUphysics.Materials.Material(materialDescription.StaticFriction, materialDescription.DynamicFriction, materialDescription.Bounciness);
instancedMesh.Sidedness = TriangleSidedness;
this.rotation = rotation;
this.scale = scale;
}
///<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 (ray.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>
/// 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 mesh.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length to test in units of the ray direction's length.</param>
///<param name="sidedness">Sidedness to use when raycasting. Doesn't have to be the same as the mesh's own sidedness.</param>
///<param name="rayHit">Data about the ray's intersection with 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)
{
return mesh.RayCast(ray, maximumLength, sidedness, out rayHit);
}
void ComputeSolidSidedness()
{
//Raycast against the mesh.
//If there's an even number of hits, then the ray start point is outside.
//If there's an odd number of hits, then the ray start point is inside.
//If the start is outside, then take the earliest toi hit and calibrate sidedness based on it.
//If the start is inside, then take the latest toi hit and calibrate sidedness based on it.
//This test assumes consistent winding across the entire mesh as well as a closed surface.
//If those assumptions are not correct, then the raycast cannot determine inclusion or exclusion,
//or there exists no calibration that will work across the entire surface.
//Pick a ray direction that goes to a random location on the mesh.
//A vertex would work, but targeting the middle of a triangle avoids some edge cases.
var ray = new Ray();
Vector3 vA, vB, vC;
triangleMesh.Data.GetTriangle(((triangleMesh.Data.indices.Length / 3) / 2) * 3, out vA, out vB, out vC);
vA.Add2( ref vB, ref vC, out vA );
vA.Mult( 1 / 3, out ray.Direction );
ray.Direction.Normalize();
SidednessWhenSolid = ComputeSolidSidednessHelper(ray);
//ComputeSolidSidednessHelper is separated into another function just in case multiple queries were desired for validation.
//If multiple rays returned different sidednesses, the shape would be inconsistent.
}
///<summary>
/// Constructs a new mobile mesh shape from cached data.
///</summary>
///<param name="meshData">Mesh data reprsenting the shape. Should already be properly centered.</param>
/// <param name="hullVertices">Outer hull vertices of the mobile mesh shape used to quickly compute the bounding box.</param>
///<param name="solidity">Solidity state of the shape.</param>
/// <param name="sidednessWhenSolid">Triangle sidedness to use when the shape is solid.</param>
/// <param name="collisionMargin">Collision margin used to expand the mesh triangles.</param>
/// <param name="volumeDescription">Description of the volume and its distribution in the shape. Assumed to be correct; no processing or validation is performed.</param>
public MobileMeshShape(TransformableMeshData meshData, IList<Vector3> hullVertices, MobileMeshSolidity solidity, TriangleSidedness sidednessWhenSolid, float collisionMargin, EntityShapeVolumeDescription volumeDescription)
{
triangleMesh = new TriangleMesh(meshData);
this.hullVertices = new RawList<Vector3>(hullVertices);
meshCollisionMargin = collisionMargin;
this.solidity = solidity;
SidednessWhenSolid = sidednessWhenSolid;
UpdateEntityShapeVolume(volumeDescription);
}
/// <summary>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="sidedness">Sidedness of the triangle to test.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit)
{
hit = new RayHit();
Vector3 ab = b - a;
Vector3 ac = c - a;
Vector3x.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
{
return(false); //Degenerate triangle!
}
float d = -Vector3.Dot(ray.Direction, hit.Normal);
switch (sidedness)
{
case TriangleSidedness.DoubleSided:
if (d <= 0) //Pointing the wrong way. Flip the normal.
{
hit.Normal = -hit.Normal;
d = -d;
}
break;
case TriangleSidedness.Clockwise:
if (d <= 0) //Pointing the wrong way. Can't hit.
{
return(false);
}
break;
case TriangleSidedness.Counterclockwise:
if (d >= 0) //Pointing the wrong way. Can't hit.
{
return(false);
}
hit.Normal = -hit.Normal;
d = -d;
break;
}
Vector3 ap = ray.Position - a;
hit.T = Vector3.Dot(ap, hit.Normal);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
{
return(false);//Hit is behind origin, or too far away.
}
hit.Location = ray.Position + hit.T * ray.Direction;
// Compute barycentric coordinates
ap = hit.Location - a;
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
ABdotAB = Vector3.Dot(ab, ab);
ABdotAC = Vector3.Dot(ab, ac);
ABdotAP = Vector3.Dot(ab, ap);
ACdotAC = Vector3.Dot(ac, ac);
ACdotAP = Vector3.Dot(ac, ap);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return((u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon));
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="rayHit">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, TriangleSidedness sidedness, out RayHit rayHit)
{
return(RayCast(ray, float.MaxValue, sidedness, out rayHit));
}
void ComputeSolidSidedness()
{
//Raycast against the mesh.
//If there's an even number of hits, then the ray start point is outside.
//If there's an odd number of hits, then the ray start point is inside.
//If the start is outside, then take the earliest toi hit and calibrate sidedness based on it.
//If the start is inside, then take the latest toi hit and calibrate sidedness based on it.
//This test assumes consistent winding across the entire mesh as well as a closed surface.
//If those assumptions are not correct, then the raycast cannot determine inclusion or exclusion,
//or there exists no calibration that will work across the entire surface.
//Pick a ray direction that goes to a random location on the mesh.
//A vertex would work, but targeting the middle of a triangle avoids some edge cases.
Ray ray = new Ray();
Vector3 vA, vB, vC;
triangleMesh.Data.GetTriangle(((triangleMesh.Data.indices.Length / 3) / 2) * 3, out vA, out vB, out vC);
ray.Direction = (vA + vB + vC) / 3;
ray.Direction.Normalize();
solidSidedness = ComputeSolidSidednessHelper(ray);
//TODO: Positions need to be valid for the verifying directions to work properly.
////Find another direction and test it to corroborate the first test.
//Ray alternateRay;
//alternateRay.Position = ray.Position;
//Vector3.Cross(ref ray.Direction, ref Toolbox.UpVector, out alternateRay.Direction);
//float lengthSquared = alternateRay.Direction.LengthSquared();
//if (lengthSquared < Toolbox.Epsilon)
//{
// Vector3.Cross(ref ray.Direction, ref Toolbox.RightVector, out alternateRay.Direction);
// lengthSquared = alternateRay.Direction.LengthSquared();
//}
//Vector3.Divide(ref alternateRay.Direction, (float)Math.Sqrt(lengthSquared), out alternateRay.Direction);
//var sidednessCandidate2 = ComputeSolidSidednessHelper(alternateRay);
//if (sidednessCandidate == sidednessCandidate2)
//{
// //The two tests agreed! It's very likely that the sidedness is, in fact, in this direction.
// solidSidedness = sidednessCandidate;
//}
//else
//{
// //The two tests disagreed. Tiebreaker!
// Vector3.Cross(ref alternateRay.Direction, ref ray.Direction, out alternateRay.Direction);
// solidSidedness = ComputeSolidSidednessHelper(alternateRay);
//}
}
/// <summary>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="sidedness">Sidedness of the triangle to test.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit)
{
hit = new RayHit();
Vector3 ab, ac;
Vector3.Subtract(ref b, ref a, out ab);
Vector3.Subtract(ref c, ref a, out ac);
Vector3.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
return false; //Degenerate triangle!
float d;
Vector3.Dot(ref ray.Direction, ref hit.Normal, out d);
d = -d;
switch (sidedness)
{
case TriangleSidedness.DoubleSided:
if (d <= 0) //Pointing the wrong way. Flip the normal.
{
Vector3.Negate(ref hit.Normal, out hit.Normal);
d = -d;
}
break;
case TriangleSidedness.Clockwise:
if (d <= 0) //Pointing the wrong way. Can't hit.
return false;
break;
case TriangleSidedness.Counterclockwise:
if (d >= 0) //Pointing the wrong way. Can't hit.
return false;
Vector3.Negate(ref hit.Normal, out hit.Normal);
d = -d;
break;
}
Vector3 ap;
Vector3.Subtract(ref ray.Position, ref a, out ap);
Vector3.Dot(ref ap, ref hit.Normal, out hit.T);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
return false;//Hit is behind origin, or too far away.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);
// Compute barycentric coordinates
Vector3.Subtract(ref hit.Location, ref a, out ap);
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
Vector3.Dot(ref ab, ref ab, out ABdotAB);
Vector3.Dot(ref ab, ref ac, out ABdotAC);
Vector3.Dot(ref ab, ref ap, out ABdotAP);
Vector3.Dot(ref ac, ref ac, out ACdotAC);
Vector3.Dot(ref ac, ref ap, out ACdotAP);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, TriangleSidedness sidedness, IList<RayHit> hits)
{
return RayCast(ray, float.MaxValue, sidedness, hits);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="rayHit">Hit data for the ray, 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)
{
var rayHits = CommonResources.GetRayHitList();
bool toReturn = RayCast(ray, maximumLength, sidedness, rayHits);
if (toReturn)
{
rayHit = rayHits[0];
for (int i = 1; i < rayHits.Count; i++)
{
RayHit hit = rayHits[i];
if (hit.T < rayHit.T)
rayHit = hit;
}
}
else
rayHit = new RayHit();
CommonResources.GiveBack(rayHits);
return toReturn;
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, IList<RayHit> hits)
{
var hitElements = CommonResources.GetIntList();
tree.GetOverlaps(ray, maximumLength, hitElements);
for (int i = 0; i < hitElements.Count; i++)
{
System.Numerics.Vector3 v1, v2, v3;
data.GetTriangle(hitElements[i], out v1, out v2, out v3);
RayHit hit;
if (Toolbox.FindRayTriangleIntersection(ref ray, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit))
{
hits.Add(hit);
}
}
CommonResources.GiveBack(hitElements);
return hits.Count > 0;
}
///<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;
}
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 (ray.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>
/// Determines the intersection between a ray and a triangle.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
/// <param name="sidedness">Sidedness of the triangle to test.</param>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="hit">Hit data of the ray, if any</param>
/// <returns>Whether or not the ray and triangle intersect.</returns>
public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit)
{
hit = new RayHit();
Vector3 ab = b - a;
Vector3 ac = c - a;
Vector3x.Cross(ref ab, ref ac, out hit.Normal);
if (hit.Normal.LengthSquared() < Epsilon)
return false; //Degenerate triangle!
float d = -Vector3.Dot(ray.Direction, hit.Normal);
switch (sidedness)
{
case TriangleSidedness.DoubleSided:
if (d <= 0) //Pointing the wrong way. Flip the normal.
{
hit.Normal = -hit.Normal;
d = -d;
}
break;
case TriangleSidedness.Clockwise:
if (d <= 0) //Pointing the wrong way. Can't hit.
return false;
break;
case TriangleSidedness.Counterclockwise:
if (d >= 0) //Pointing the wrong way. Can't hit.
return false;
hit.Normal = -hit.Normal;
d = -d;
break;
}
Vector3 ap = ray.Position - a;
hit.T = Vector3.Dot(ap, hit.Normal);
hit.T /= d;
if (hit.T < 0 || hit.T > maximumLength)
return false;//Hit is behind origin, or too far away.
hit.Location = ray.Position + hit.T * ray.Direction;
// Compute barycentric coordinates
ap = hit.Location - a;
float ABdotAB, ABdotAC, ABdotAP;
float ACdotAC, ACdotAP;
ABdotAB = Vector3.Dot(ab, ab);
ABdotAC = Vector3.Dot(ab, ac);
ABdotAP = Vector3.Dot(ab, ap);
ACdotAC = Vector3.Dot(ac, ac);
ACdotAP = Vector3.Dot(ac, ap);
float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;
return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon);
}
