protected override void ComponentsCreatedHandler(object sender, EventArgs e)
{
base.ComponentsCreatedHandler(sender, e);
TerrainRenderComponent terrainRenderComponent = Owner.GetComponent<TerrainRenderComponent>(ComponentType.Render);
if (terrainRenderComponent == null)
throw new LevelManifestException("TerrainCollisionComponent expect to be accompanied by a TerrainRenderComponent.");
float[,] heightVals = terrainRenderComponent.Heights;
XnaVector3 originShift = new XnaVector3(terrainRenderComponent.TerrainAsset.XZScale * (terrainRenderComponent.TerrainAsset.VertexCountAlongXAxis - 1) * 0.5f,
0.0f,
terrainRenderComponent.TerrainAsset.XZScale * (terrainRenderComponent.TerrainAsset.VertexCountAlongZAxis - 1) * 0.5f);
AffineTransform terrainTransform = new BEPUutilities.AffineTransform(
new BEPUutilities.Vector3(terrainRenderComponent.TerrainAsset.XZScale, 1.0f, terrainRenderComponent.TerrainAsset.XZScale),
BepuConverter.Convert(mTransformComponent.Orientation),
BepuConverter.Convert(mTransformComponent.Translation - originShift));
mSimTerrain = new BepuTerrain(heightVals, terrainTransform);
mSimTerrain.Material.Bounciness = 0.60f;
mSimTerrain.Material.StaticFriction = 1.0f;
mSimTerrain.Material.KineticFriction = 1.0f;
mSimTerrain.Tag = Owner.Id;
}
protected override void ComponentsCreatedHandler(object sender, EventArgs e)
{
base.ComponentsCreatedHandler(sender, e);
TerrainRenderComponent terrainRenderComponent = Owner.GetComponent <TerrainRenderComponent>(ComponentType.Render);
if (terrainRenderComponent == null)
{
throw new LevelManifestException("TerrainCollisionComponent expect to be accompanied by a TerrainRenderComponent.");
}
float[,] heightVals = terrainRenderComponent.Heights;
XnaVector3 originShift = new XnaVector3(terrainRenderComponent.TerrainAsset.XZScale * (terrainRenderComponent.TerrainAsset.VertexCountAlongXAxis - 1) * 0.5f,
0.0f,
terrainRenderComponent.TerrainAsset.XZScale * (terrainRenderComponent.TerrainAsset.VertexCountAlongZAxis - 1) * 0.5f);
AffineTransform terrainTransform = new BEPUutilities.AffineTransform(
new BEPUutilities.Vector3(terrainRenderComponent.TerrainAsset.XZScale, 1.0f, terrainRenderComponent.TerrainAsset.XZScale),
BepuConverter.Convert(mTransformComponent.Orientation),
BepuConverter.Convert(mTransformComponent.Translation - originShift));
mSimTerrain = new BepuTerrain(heightVals, terrainTransform);
mSimTerrain.Material.Bounciness = 0.60f;
mSimTerrain.Material.StaticFriction = 1.0f;
mSimTerrain.Material.KineticFriction = 1.0f;
mSimTerrain.Tag = Owner.Id;
}
///<summary>
/// Constructs the mesh data.
///</summary>
///<param name="vertices">Vertice sto use in the mesh data.</param>
///<param name="indices">Indices to use in the mesh data.</param>
///<param name="worldTransform">Transform to apply to vertices before returning their positions.</param>
public TransformableMeshData(Vector3[] vertices, uint[] indices, int indexCount, AffineTransform worldTransform)
{
this.worldTransform = worldTransform;
Vertices = vertices;
Indices = indices;
IndexCount = indexCount;
}
///<summary>
/// Constructs a new Terrain.
///</summary>
///<param name="shape">Shape to use for the terrain.</param>
///<param name="worldTransform">Transform to use for the terrain.</param>
public Terrain(TerrainShape shape, AffineTransform worldTransform)
{
WorldTransform = worldTransform;
Shape = shape;
Events = new ContactEventManager<Terrain>();
}
///<summary>
/// Constructs a new InstancedMesh.
///</summary>
///<param name="meshShape">Shape to use for the instance.</param>
///<param name="worldTransform">Transform to use for the instance.</param>
public InstancedMesh(InstancedMeshShape meshShape, AffineTransform worldTransform)
{
this.worldTransform = worldTransform;
base.Shape = meshShape;
Events = new ContactEventManager<InstancedMesh>();
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox);
var tri = PhysicsResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.maximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.maximumRadius < radius)
{
tri.maximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.maximumRadius < radius)
{
tri.maximumRadius = radius;
}
tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.maximumRadius = 0;
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MobileMeshDemo(DemosGame game)
: base(game)
{
Vector3[] vertices;
int[] indices;
//Create a big hollow sphere (squished into an ellipsoid).
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load<Model>("hollowsphere"), out vertices, out indices);
var transform = new AffineTransform(new Vector3(.06f, .04f, .06f), Quaternion.Identity, new Vector3(0, 0, 0));
//Note that meshes can also be made solid (MobileMeshSolidity.Solid). This gives meshes a solid collidable volume, instead of just
//being thin shells. However, enabling solidity is more expensive.
var mesh = new MobileMesh(vertices, indices, transform, MobileMeshSolidity.Counterclockwise);
mesh.Position = new Vector3(0, 0, 0);
//Make the mesh spin a bit!
mesh.AngularVelocity = new Vector3(0, 1, 0);
Space.Add(mesh);
//Add another mobile mesh inside.
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load<Model>("tube"), out vertices, out indices);
transform = new AffineTransform(new Vector3(1, 1, 1), Quaternion.Identity, new Vector3(0, 0, 0));
mesh = new MobileMesh(vertices, indices, transform, MobileMeshSolidity.Counterclockwise, 10);
mesh.Position = new Vector3(0, 10, 0);
Space.Add(mesh);
//Create a bunch of boxes.
#if WINDOWS
int numColumns = 5;
int numRows = 5;
int numHigh = 5;
#else
//Keep the simulation a bit smaller on the xbox.
int numColumns = 4;
int numRows = 4;
int numHigh = 4;
#endif
float separation = 1.5f;
for (int i = 0; i < numRows; i++)
for (int j = 0; j < numColumns; j++)
for (int k = 0; k < numHigh; k++)
{
Space.Add(new Box(new Vector3(separation * i, k * separation, separation * j), 1, 1, 1, 5));
}
//Space.Add(new Box(new Vector3(0, -10, 0), 1, 1, 1));
game.Camera.Position = new Vector3(0, -10, 5);
}
///<summary>
/// Constructs a new mobile mesh shape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
///<param name="localTransform">Local transform to apply to the shape.</param>
///<param name="solidity">Solidity state of the shape.</param>
public MobileMeshShape(System.Numerics.Vector3[] vertices, int[] indices, AffineTransform localTransform, MobileMeshSolidity solidity)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, localTransform);
var shapeDistributionInformation = ComputeVolumeDistribution(data);
data.worldTransform.Translation -= shapeDistributionInformation.Center;
triangleMesh = new TriangleMesh(data);
UpdateEntityShapeVolume(new EntityShapeVolumeDescription { Volume = shapeDistributionInformation.Volume, VolumeDistribution = shapeDistributionInformation.VolumeDistribution });
ComputeSolidSidedness();
UpdateSurfaceVertices();
}
///<summary>
/// Constructs a new mobile mesh shape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
///<param name="localTransform">Local transform to apply to the shape.</param>
///<param name="solidity">Solidity state of the shape.</param>
public MobileMeshShape(Vector3[] vertices, int[] indices, AffineTransform localTransform, MobileMeshSolidity solidity)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, localTransform);
ShapeDistributionInformation distributionInfo;
ComputeShapeInformation(data, out distributionInfo);
for (int i = 0; i < surfaceVertices.Count; i++)
{
Vector3.Subtract(ref surfaceVertices.Elements[i], ref distributionInfo.Center, out surfaceVertices.Elements[i]);
}
triangleMesh = new TriangleMesh(data);
ComputeSolidSidedness();
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public InstancedMeshDemo(DemosGame game)
: base(game)
{
Vector3[] vertices;
int[] indices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load<Model>("guy"), out vertices, out indices);
var meshShape = new InstancedMeshShape(vertices, indices);
var random = new Random();
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 10; j++)
{
//Create a transform and the instance of the mesh.
var transform = new AffineTransform(
new Vector3((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f),
Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100),
new Vector3(i * 2, 3, j * 2));
var mesh = new InstancedMesh(meshShape, transform);
//Making the triangles one-sided makes collision detection a bit more robust, since the backsides of triangles won't try to collide with things
//and 'pull' them back into the mesh.
mesh.Sidedness = TriangleSidedness.Counterclockwise;
Space.Add(mesh);
game.ModelDrawer.Add(mesh);
}
}
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
//Drop a box on the mesh.
Space.Add(new Box(new Vector3((i + 1) * 4, 10, (j + 1) * 4), 1, 1, 1, 10));
}
}
Space.Add(new Box(new Vector3(10, 0, 10), 20, 1, 20));
game.Camera.Position = new Vector3(10, 6, 30);
}
///<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>
/// Computes the bounding box of the transformed mesh shape.
///</summary>
///<param name="transform">Transform to apply to the shape during the bounding box calculation.</param>
///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param>
public void ComputeBoundingBox(ref AffineTransform transform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
float minX = float.MaxValue;
float minY = float.MaxValue;
float minZ = float.MaxValue;
float maxX = -float.MaxValue;
float maxY = -float.MaxValue;
float maxZ = -float.MaxValue;
for (int i = 0; i < triangleMesh.Data.vertices.Length; i++)
{
Vector3 vertex;
triangleMesh.Data.GetVertexPosition(i, out vertex);
Matrix3x3.Transform(ref vertex, ref transform.LinearTransform, out vertex);
if (vertex.X < minX)
minX = vertex.X;
if (vertex.X > maxX)
maxX = vertex.X;
if (vertex.Y < minY)
minY = vertex.Y;
if (vertex.Y > maxY)
maxY = vertex.Y;
if (vertex.Z < minZ)
minZ = vertex.Z;
if (vertex.Z > maxZ)
maxZ = vertex.Z;
}
boundingBox.Min.X = transform.Translation.X + minX;
boundingBox.Min.Y = transform.Translation.Y + minY;
boundingBox.Min.Z = transform.Translation.Z + minZ;
boundingBox.Max.X = transform.Translation.X + maxX;
boundingBox.Max.Y = transform.Translation.Y + maxY;
boundingBox.Max.Z = transform.Translation.Z + maxZ;
}
//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;
}
//Transform the convex into the space of something else.
/// <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="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this.
//Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can
//slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case).
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(transform.LinearTransform.M11, transform.LinearTransform.M21, transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out right);
Vector3 left;
direction = new Vector3(-transform.LinearTransform.M11, -transform.LinearTransform.M21, -transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out left);
Vector3 up;
direction = new Vector3(transform.LinearTransform.M12, transform.LinearTransform.M22, transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out up);
Vector3 down;
direction = new Vector3(-transform.LinearTransform.M12, -transform.LinearTransform.M22, -transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out down);
Vector3 backward;
direction = new Vector3(transform.LinearTransform.M13, transform.LinearTransform.M23, transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out backward);
Vector3 forward;
direction = new Vector3(-transform.LinearTransform.M13, -transform.LinearTransform.M23, -transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out forward);
//This could be optimized. Unnecessary transformation information gets computed.
Matrix3x3.Transform(ref right, ref transform.LinearTransform, out right);
Matrix3x3.Transform(ref left, ref transform.LinearTransform, out left);
Matrix3x3.Transform(ref up, ref transform.LinearTransform, out up);
Matrix3x3.Transform(ref down, ref transform.LinearTransform, out down);
Matrix3x3.Transform(ref backward, ref transform.LinearTransform, out backward);
Matrix3x3.Transform(ref forward, ref transform.LinearTransform, out forward);
//These right/up/backward represent the extreme points in world space along the world space axes.
boundingBox.Max.X = transform.Translation.X + right.X;
boundingBox.Max.Y = transform.Translation.Y + up.Y;
boundingBox.Max.Z = transform.Translation.Z + backward.Z;
boundingBox.Min.X = transform.Translation.X + left.X;
boundingBox.Min.Y = transform.Translation.Y + down.Y;
boundingBox.Min.Z = transform.Translation.Z + forward.Z;
}
/// <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);
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform, out AffineTransform fromMeshLocalToConvexLocal)
{
//MobileMeshes only have TransformableMeshData sources.
var data = ((TransformableMeshData)mesh.Shape.TriangleMesh.Data);
//The mobile mesh has a shape-based transform followed by the rigid body transform.
AffineTransform mobileMeshWorldTransform;
AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out mobileMeshWorldTransform);
AffineTransform combinedMobileMeshWorldTransform;
AffineTransform.Multiply(ref data.worldTransform, ref mobileMeshWorldTransform, out combinedMobileMeshWorldTransform);
AffineTransform.Multiply(ref combinedMobileMeshWorldTransform, ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
}
/// <summary>
/// Gets the world space normal at the given indices.
/// </summary>
///<param name="i">Index in the first dimension.</param>
///<param name="j">Index in the second dimension.</param>
/// <param name="transform">Transform to apply to the terrain while computing the normal.</param>
/// <param name="normal">World space normal at the given indices.</param>
public void GetNormal(int i, int j, ref AffineTransform transform, out Vector3 normal)
{
Vector3 top;
Vector3 bottom;
Vector3 right;
Vector3 left;
if (i <= 0)
i = 0;
else if (i >= heights.GetLength(0))
i = heights.GetLength(0) - 1;
if (j <= 0)
j = 0;
else if (j >= heights.GetLength(1))
j = heights.GetLength(1) - 1;
GetPosition(i, Math.Min(j + 1, heights.GetLength(1) - 1), ref transform, out top);
GetPosition(i, Math.Max(j - 1, 0), ref transform, out bottom);
GetPosition(Math.Min(i + 1, heights.GetLength(0) - 1), j, ref transform, out right);
GetPosition(Math.Max(i - 1, 0), j, ref transform, out left);
Vector3 temp;
Vector3.Subtract(ref top, ref bottom, out temp);
Vector3.Subtract(ref right, ref left, out normal);
Vector3.Cross(ref temp, ref normal, out normal);
normal.Normalize();
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MutableStaticGroupTestDemo(DemosGame game)
: base(game)
{
//Creating a bunch of separate StaticMeshes or kinematic Entity objects for an environment can pollute the broad phase.
//This is because the broad phase implementation doesn't have guarantees about what elements can collide, so it has to
//traverse the acceleration structure all the way down to pairs to figure it out. That can get expensive!
//Individual objects, like StaticMeshes, can have very complicated geometry without hurting the broad phase because the broad phase
//has no knowledge of the thousands of triangles in the mesh. The StaticMesh itself knows that the triangles within the mesh
//never need to collide, so it never needs to test them against each other.
//Similarly, the StaticGroup can be given a bunch of separate collidables. The broad phase doesn't directly know about these child collidables-
//it only sees the StaticGroup. The StaticGroup knows that the things inside it can't ever collide with each other, so no tests are needed.
//This avoids the performance problem!
//To demonstrate, we'll be creating a set of static objects and giving them to a group to manage.
var collidables = new List<Collidable>();
//Start with a whole bunch of boxes. These are entity collidables, but without entities!
float xSpacing = 6;
float ySpacing = 6;
float zSpacing = 6;
//NOTE: You might notice this demo takes a while to start, especially on the Xbox360. Do not fear! That's due to the creation of the graphics data, not the physics.
//The physics can handle over 100,000 static objects pretty easily. The graphics, not so much :)
//Try disabling the game.ModelDrawer.Add() lines and increasing the number of static objects.
int xCount = 15;
int yCount = 7;
int zCount = 15;
var random = new Random(5);
for (int i = 0; i < xCount; i++)
{
for (int j = 0; j < yCount; j++)
{
for (int k = 0; k < zCount; k++)
{
//Create a transform and the instance of the mesh.
var collidable = new ConvexCollidable<BoxShape>(new BoxShape((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f));
//This EntityCollidable isn't associated with an entity, so we must manually tell it where to sit by setting the WorldTransform.
//This also updates its bounding box.
collidable.WorldTransform = new RigidTransform(
new Vector3(i * xSpacing - xCount * xSpacing * .5f, j * ySpacing + 3, k * zSpacing - zCount * zSpacing * .5f),
Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100));
collidables.Add(collidable);
}
}
}
//Now create a bunch of instanced meshes too.
xSpacing = 6;
ySpacing = 6;
zSpacing = 6;
xCount = 10;
yCount = 2;
zCount = 10;
Vector3[] vertices;
int[] indices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load<Model>("fish"), out vertices, out indices);
var meshShape = new InstancedMeshShape(vertices, indices);
for (int i = 0; i < xCount; i++)
{
for (int j = 0; j < yCount; j++)
{
for (int k = 0; k < zCount; k++)
{
//Create a transform and the instance of the mesh.
var transform = new AffineTransform(
new Vector3((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f),
Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100),
new Vector3(i * xSpacing - xCount * xSpacing * .5f, j * ySpacing + 50, k * zSpacing - zCount * zSpacing * .5f));
var mesh = new InstancedMesh(meshShape, transform);
//Making the triangles one-sided makes collision detection a bit more robust, since the backsides of triangles won't try to collide with things
//and 'pull' them back into the mesh.
mesh.Sidedness = TriangleSidedness.Counterclockwise;
collidables.Add(mesh);
}
}
}
var ground = new ConvexCollidable<BoxShape>(new BoxShape(200, 1, 200));
ground.WorldTransform = new RigidTransform(new Vector3(0, -3, 0), Quaternion.Identity);
collidables.Add(ground);
var group = new StaticGroup(collidables);
var removed = new RawList<Collidable>();
var contained = new RawList<Collidable>();
group.Shape.CollidableTree.CollectLeaves(contained);
for (int i = 0; i < 100000; ++i)
{
for (int collidableIndex = contained.Count - 1; collidableIndex >= 0; --collidableIndex)
{
if (random.NextDouble() < 0.6)
{
group.Shape.Remove(contained[collidableIndex]);
removed.Add(contained[collidableIndex]);
contained.FastRemoveAt(collidableIndex);
}
}
for (int collidableIndex = removed.Count - 1; collidableIndex >= 0; --collidableIndex)
{
if (random.NextDouble() < 0.4)
{
group.Shape.Add(removed[collidableIndex]);
contained.Add(removed[collidableIndex]);
removed.FastRemoveAt(collidableIndex);
}
}
}
for (int i = 0; i < contained.Count; ++i)
{
game.ModelDrawer.Add(contained[i]);
}
Space.Add(group);
//Create a bunch of dynamic boxes to drop on the staticswarm.
xCount = 8;
yCount = 3;
zCount = 8;
xSpacing = 3f;
ySpacing = 5f;
zSpacing = 3f;
for (int i = 0; i < xCount; i++)
for (int j = 0; j < zCount; j++)
for (int k = 0; k < yCount; k++)
{
Space.Add(new Box(new Vector3(
xSpacing * i - (xCount - 1) * xSpacing / 2f,
100 + k * (ySpacing),
2 + zSpacing * j - (zCount - 1) * zSpacing / 2f),
1, 1, 1, 10));
}
game.Camera.Position = new Vector3(0, 60, 90);
}
public static void Validate(this AffineTransform a)
{
a.LinearTransform.Validate();
a.Translation.Validate();
}
///<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="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, out RayHit hit)
{
return RayCast(ref ray, maximumLength, ref transform, TriangleSidedness.Counterclockwise, out hit);
}
///<summary>
/// Gets a world space triangle in the terrain at the given indices (as if it were a mesh).
///</summary>
///<param name="indices">Indices of the triangle.</param>
///<param name="transform">Transform to apply to the triangle vertices.</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>
public void GetTriangle(ref TriangleMeshConvexContactManifold.TriangleIndices indices, ref AffineTransform transform, out Vector3 a, out Vector3 b, out Vector3 c)
{
//Reverse the encoded index:
//index = i + width * j
int width = heights.GetLength(0);
int columnA = indices.A / width;
int rowA = indices.A - columnA * width;
int columnB = indices.B / width;
int rowB = indices.B - columnB * width;
int columnC = indices.C / width;
int rowC = indices.C - columnC * width;
GetPosition(rowA, columnA, ref transform, out a);
GetPosition(rowB, columnB, ref transform, out b);
GetPosition(rowC, columnC, ref transform, out c);
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform, out AffineTransform fromMeshLocalToConvexLocal)
{
//InstancedMeshShapes don't have a shape-level transform. The instance transform is all there is.
AffineTransform.Multiply(ref mesh.worldTransform, ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
}
///<summary>
/// Constructs a new StaticMeshShape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
///<param name="worldTransform">World transform to use in the local space data.</param>
public StaticMeshShape(Vector3[] vertices, int[] indices, AffineTransform worldTransform)
{
triangleMeshData = new TransformableMeshData(vertices, indices, worldTransform);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray() { Position = startingTransform.Position, Direction = Toolbox.UpVector };
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit() { Location = startingTransform.Position, Normal = new Vector3(), T = 0 };
return true;
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {Translation = worldTransform.Position};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.maximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.maximumRadius < radius)
tri.maximumRadius = radius;
radius = tri.vC.LengthSquared();
if (tri.maximumRadius < radius)
tri.maximumRadius = radius;
tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {Orientation = Quaternion.Identity, Position = center};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.maximumRadius = 0;
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return false;
}
///<summary>
/// Constructs a new InstancedMesh.
///</summary>
///<param name="meshShape">Shape to use for the instance.</param>
///<param name="worldTransform">Transform to use for the instance.</param>
public InstancedMesh(InstancedMeshShape meshShape, AffineTransform worldTransform)
{
this.worldTransform = worldTransform;
base.Shape = meshShape;
Events = new ContactEventManager <InstancedMesh>();
}
///<summary>
/// Constructs the mesh data.
///</summary>
///<param name="vertices">Vertice sto use in the mesh data.</param>
///<param name="indices">Indices to use in the mesh data.</param>
///<param name="worldTransform">Transform to apply to vertices before returning their positions.</param>
public TransformableMeshData(System.Numerics.Vector3[] vertices, int[] indices, AffineTransform worldTransform)
{
this.worldTransform = worldTransform;
Vertices = vertices;
Indices = indices;
}
///<summary>
/// Constructs a new static mesh.
///</summary>
///<param name="vertices">Vertex positions of the mesh.</param>
///<param name="indices">Index list of the mesh.</param>
/// <param name="worldTransform">Transform to use to create the mesh initially.</param>
public StaticMesh(Vector3[] vertices, int[] indices, AffineTransform worldTransform)
{
base.Shape = new StaticMeshShape(vertices, indices, worldTransform);
Events = new ContactEventManager<StaticMesh>();
}
///<summary>
/// Constructs the bounding box of the terrain given a transform.
///</summary>
///<param name="transform">Transform to apply to the terrain during the bounding box calculation.</param>
///<param name="boundingBox">Bounding box of the terrain shape when transformed.</param>
public void GetBoundingBox(ref AffineTransform transform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
float minX = float.MaxValue, maxX = -float.MaxValue,
minY = float.MaxValue, maxY = -float.MaxValue,
minZ = float.MaxValue, maxZ = -float.MaxValue;
Vector3 minXvertex = new Vector3(),
maxXvertex = new Vector3(),
minYvertex = new Vector3(),
maxYvertex = new Vector3(),
minZvertex = new Vector3(),
maxZvertex = new Vector3();
//Find the extreme locations.
for (int i = 0; i < heights.GetLength(0); i++)
{
for (int j = 0; j < heights.GetLength(1); j++)
{
var vertex = new Vector3(i, heights[i, j], j);
Matrix3x3.Transform(ref vertex, ref transform.LinearTransform, out vertex);
if (vertex.X < minX)
{
minX = vertex.X;
minXvertex = vertex;
}
else if (vertex.X > maxX)
{
maxX = vertex.X;
maxXvertex = vertex;
}
if (vertex.Y < minY)
{
minY = vertex.Y;
minYvertex = vertex;
}
else if (vertex.Y > maxY)
{
maxY = vertex.Y;
maxYvertex = vertex;
}
if (vertex.Z < minZ)
{
minZ = vertex.Z;
minZvertex = vertex;
}
else if (vertex.Z > maxZ)
{
maxZ = vertex.Z;
maxZvertex = vertex;
}
}
}
//Shift the bounding box.
boundingBox.Min.X = minXvertex.X + transform.Translation.X;
boundingBox.Min.Y = minYvertex.Y + transform.Translation.Y;
boundingBox.Min.Z = minZvertex.Z + transform.Translation.Z;
boundingBox.Max.X = maxXvertex.X + transform.Translation.X;
boundingBox.Max.Y = maxYvertex.Y + transform.Translation.Y;
boundingBox.Max.Z = maxZvertex.Z + transform.Translation.Z;
}
//Transform the convex into the space of something else.
/// <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="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this. At the very least, it should be possible to avoid the 6 square roots involved currently.
//If this shows a a bottleneck, it might be best to virtualize this function and implement a per-shape variant.
//Also... It might be better just to have the internal function be a GetBoundingBox that takes an AffineTransform, and an outer function
//does the local space fiddling.
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(transform.LinearTransform.M11, transform.LinearTransform.M21, transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out right);
Vector3 left;
direction = new Vector3(-transform.LinearTransform.M11, -transform.LinearTransform.M21, -transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out left);
Vector3 up;
direction = new Vector3(transform.LinearTransform.M12, transform.LinearTransform.M22, transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out up);
Vector3 down;
direction = new Vector3(-transform.LinearTransform.M12, -transform.LinearTransform.M22, -transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out down);
Vector3 backward;
direction = new Vector3(transform.LinearTransform.M13, transform.LinearTransform.M23, transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out backward);
Vector3 forward;
direction = new Vector3(-transform.LinearTransform.M13, -transform.LinearTransform.M23, -transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref transform.LinearTransform, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref transform.LinearTransform, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = transform.Translation.X + positive.X;
boundingBox.Max.Y = transform.Translation.Y + positive.Y;
boundingBox.Max.Z = transform.Translation.Z + positive.Z;
boundingBox.Min.X = transform.Translation.X + negative.X;
boundingBox.Min.Y = transform.Translation.Y + negative.Y;
boundingBox.Min.Z = transform.Translation.Z + negative.Z;
}
/// <summary>
/// Gets the world space position of a vertex in the terrain at the given indices.
/// </summary>
///<param name="i">Index in the first dimension.</param>
///<param name="j">Index in the second dimension.</param>
/// <param name="transform">Transform to apply to the vertex.</param>
/// <param name="position">Transformed position of the vertex at the given indices.</param>
public void GetPosition(int i, int j, ref AffineTransform transform, out Vector3 position)
{
if (i <= 0)
i = 0;
else if (i >= heights.GetLength(0))
i = heights.GetLength(0) - 1;
if (j <= 0)
j = 0;
else if (j >= heights.GetLength(1))
j = heights.GetLength(1) - 1;
#if !WINDOWS
position = new Vector3();
#endif
position.X = i;
position.Y = heights[i, j];
position.Z = j;
AffineTransform.Transform(ref position, ref transform, out position);
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform, out AffineTransform fromMeshLocalToConvexLocal)
{
//StaticMeshes only have transformable mesh data.
var data = ((TransformableMeshData) mesh.Mesh.Data);
AffineTransform.Multiply(ref data.worldTransform, ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
}
///<summary>
/// Gets a world space triangle in the terrain at the given triangle index.
///</summary>
///<param name="index">Index of the triangle.</param>
///<param name="transform">Transform to apply to the triangle vertices.</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>
public void GetTriangle(int index, ref AffineTransform transform, out Vector3 a, out Vector3 b, out Vector3 c)
{
//Find the quad.
int quadIndex = index / 2;
bool isFirstTriangle = quadIndex * 2 == index;
int column = quadIndex / heights.GetLength(0);
int row = quadIndex - column * heights.GetLength(0);
if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomLeftUpperRight)
{
if (isFirstTriangle)
{
GetPosition(row, column, ref transform, out a);
GetPosition(row + 1, column, ref transform, out b);
GetPosition(row, column + 1, ref transform, out c);
}
else
{
GetPosition(row, column + 1, ref transform, out a);
GetPosition(row + 1, column + 1, ref transform, out b);
GetPosition(row + 1, column, ref transform, out c);
}
}
else
{
//The quad is BottomRightUpperLeft.
if (isFirstTriangle)
{
GetPosition(row, column, ref transform, out a);
GetPosition(row + 1, column, ref transform, out b);
GetPosition(row + 1, column + 1, ref transform, out c);
}
else
{
GetPosition(row, column, ref transform, out a);
GetPosition(row, column + 1, ref transform, out b);
GetPosition(row + 1, column + 1, ref transform, out c);
}
}
}
///<summary>
/// Constructs a new Terrain.
///</summary>
///<param name="heights">Height data to use to create the TerrainShape.</param>
///<param name="worldTransform">Transform to use for the terrain.</param>
public Terrain(float[,] heights, AffineTransform worldTransform)
: this(new TerrainShape(heights), worldTransform)
{
}
///<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>
/// Gets the bounding box of the mesh 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="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this.
//Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can
//slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case).
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
GetBoundingBox(ref transform.LinearTransform, out boundingBox);
boundingBox.Max.X += transform.Translation.X;
boundingBox.Max.Y += transform.Translation.Y;
boundingBox.Max.Z += transform.Translation.Z;
boundingBox.Min.X += transform.Translation.X;
boundingBox.Min.Y += transform.Translation.Y;
boundingBox.Min.Z += transform.Translation.Z;
}