///<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>();
}
public void SetCollision()
{
AffineTransform transform = new AffineTransform(initscale, initorient, Position);
Vector3[] staticTriangleVertices;
int[] staticTriangleIndices;
TriangleMesh.GetVerticesAndIndicesFromModel(_model, out staticTriangleVertices, out staticTriangleIndices);
_physmesh = new StaticMesh(staticTriangleVertices, staticTriangleIndices, transform);
Game1.space.Add(_physmesh);
Game1.thegame.ModelDrawer.Add(_physmesh.Mesh);
}
///<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);
collisionRules.group = CollisionRules.DefaultKinematicCollisionGroup;
events = new ContactEventManager<StaticMesh>(this);
material = new Material();
materialChangedDelegate = OnMaterialChanged;
material.MaterialChanged += materialChangedDelegate;
}
///<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;
collisionRules.group = CollisionRules.DefaultKinematicCollisionGroup;
events = new ContactEventManager<InstancedMesh>(this);
material = new Material();
materialChangedDelegate = OnMaterialChanged;
material.MaterialChanged += materialChangedDelegate;
}
/// <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).
TriangleMesh.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.
TriangleMesh.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);
game.Camera.Yaw = 0;
game.Camera.Pitch = 0;
}
public void add(String file, Leoni game, AffineTransform pos)
{
//Load Model
Model model = game.Content.Load<Model>(file);
//change the model to a mesh to create vertici collision
Vector3[] vertices;
int[] indices;
TriangleMesh.GetVerticesAndIndicesFromModel(model, out vertices, out indices);
var mesh = new StaticMesh(vertices, indices, pos);
//Add it to the space!
game.space.Add(mesh);
//Make it visible too.
StaticModel tmpModel = new StaticModel(model, mesh.WorldTransform.Matrix, game);
game.Components.Add(tmpModel);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public InstancedMeshDemo(DemosGame game)
: base(game)
{
Vector3[] vertices;
int[] indices;
TriangleMesh.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);
}
public BepuEntity createWheel(Vector3 position, string mesh, float scale)
{
BepuEntity entity = new BepuEntity();
entity.modelName = "Wheels7";
entity.LoadContent();
Vector3[] vertices;
int[] indices;
TriangleMesh.GetVerticesAndIndicesFromModel(entity.model, out vertices, out indices);
AffineTransform localTransform = new AffineTransform(new Vector3(scale, scale, scale), Quaternion.Identity, new Vector3(0, 0, 0));
MobileMesh mobileMesh = new MobileMesh(vertices, indices, localTransform, BEPUphysics.CollisionShapes.MobileMeshSolidity.Counterclockwise, 1);
//Correct Scale for 'Wheels6:'
// entity.localTransform = Matrix.CreateScale(4.5f, 4.5f, 4.5f);
entity.localTransform = Matrix.CreateScale(2.75f, 2.75f, 2.75f);
entity.body = mobileMesh;
entity.HasColor = true;
entity.body.Position = position;
XNAGame.Instance().space.Add(entity.body);
XNAGame.Instance().children.Add(entity);
//modelDrawer.Add(entity.body);
return entity;
}
///<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;
}
public BepuEntity createCylinder(Vector3 position, string mesh, float scale)
{
BepuEntity entity = new BepuEntity();
entity.modelName = "SlaveCylinder";
entity.LoadContent();
entity.Mass = 0;
Vector3[] vertices;
int[] indices;
TriangleMesh.GetVerticesAndIndicesFromModel(entity.model, out vertices, out indices);
AffineTransform localTransform = new AffineTransform(new Vector3(.4f, .4f, .4f), Quaternion.Identity, new Vector3(0, 0, 0));
MobileMesh mobileMesh = new MobileMesh(vertices, indices, localTransform, BEPUphysics.CollisionShapes.MobileMeshSolidity.Counterclockwise, 1);
entity.localTransform = Matrix.CreateScale(1f, 1f,1f);
entity.body = mobileMesh;
entity.HasColor = true;
entity.body.Position = position;
entity.body.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2);
XNAGame.Instance().space.Add(entity.body);
XNAGame.Instance().children.Add(entity);
// modelDrawer.Add(entity.body);
return entity;
}
public CollisionMesh(Model _model, Vector3 _position, Quaternion _rotation)
{
model = _model;
base.Position = _position;
rotation = _rotation;
Vector3[] vertices;
int[] indices;
TriangleMesh.GetVerticesAndIndicesFromModel(_model, out vertices, out indices);
AffineTransform transform = new AffineTransform(Quaternion.Identity, Vector3.Zero);
meshCollider = new MobileMesh(vertices, indices, transform, MobileMeshSolidity.Counterclockwise);
//meshCollider = new ConvexHull(vertices);
meshCollider.CollisionInformation.Tag = this;
meshCollider.CollisionInformation.LocalPosition = meshCollider.Position;
meshCollider.Position = _position;
meshCollider.Orientation = _rotation;
meshCollider.PositionUpdateMode = PositionUpdateMode.Passive;
/*if (!groupSet)
{
meshGroup = new CollisionGroup();
CollisionGroupPair pair = new CollisionGroupPair(meshGroup, meshGroup);
CollisionRules.CollisionGroupRules.Add(pair, CollisionRule.NoBroadPhase);
groupSet = true;
}
else { }*/
// meshCollider.CollisionInformation.CollisionRules.Group = meshGroup;
#if DEBUG
if (wireFrame == null)
{
wireFrame = new RasterizerState();
wireFrame.FillMode = FillMode.WireFrame;
}
else { }
#endif
visibilityScale = new Vector3(RESCALE, RESCALE, RESCALE);
}
protected internal override int FindOverlappingTriangles(float dt)
{
BoundingBox boundingBox;
AffineTransform transform = new AffineTransform(mesh.worldTransform.Orientation, mesh.worldTransform.Position);
convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref transform, out boundingBox);
Vector3 transformedVelocity;
//Compute the relative velocity with respect to the mesh. The mesh's bounding tree is NOT expanded with velocity,
//so whatever motion there is between the two objects needs to be included in the convex's bounding box.
if (convex.entity != null)
transformedVelocity = convex.entity.linearVelocity;
else
transformedVelocity = new Vector3();
if (mesh.entity != null)
Vector3.Subtract(ref transformedVelocity, ref mesh.entity.linearVelocity, out transformedVelocity);
//The linear transform is known to be orientation only, so using the transpose is allowed.
Matrix3X3.TransformTranspose(ref transformedVelocity, ref transform.LinearTransform, out transformedVelocity);
Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity);
if (transformedVelocity.X > 0)
boundingBox.Max.X += transformedVelocity.X;
else
boundingBox.Min.X += transformedVelocity.X;
if (transformedVelocity.Y > 0)
boundingBox.Max.Y += transformedVelocity.Y;
else
boundingBox.Min.Y += transformedVelocity.Y;
if (transformedVelocity.Z > 0)
boundingBox.Max.Z += transformedVelocity.Z;
else
boundingBox.Min.Z += transformedVelocity.Z;
mesh.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, overlappedTriangles);
return overlappedTriangles.count;
}
///<summary>
/// 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++)
{
Vector3 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;
}
///<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="shape">Shape to use for the terrain.</param>
///<param name="worldTransform">Transform to use for the terrain.</param>
public Terrain(TerrainShape shape, AffineTransform worldTransform)
{
this.worldTransform = worldTransform;
Shape = shape;
collisionRules.group = CollisionRules.DefaultKinematicCollisionGroup;
material = new Material();
materialChangedDelegate = OnMaterialChanged;
material.MaterialChanged += materialChangedDelegate;
events = new ContactEventManager<Terrain>(this);
}
///<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>
/// 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, int[] indices, AffineTransform worldTransform)
{
this.worldTransform = worldTransform;
Vertices = vertices;
Indices = indices;
}
/// <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)
{
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;
AffineTransform transform = new AffineTransform();
transform.Translation = worldTransform.Position;
Matrix3X3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = Resources.GetTriangle();
var hitElements = Resources.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();
triangleTransform.Orientation = Quaternion.Identity;
triangleTransform.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;
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
Resources.GiveBack(tri);
Resources.GiveBack(hitElements);
return false;
}
/// <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="sweep">World space sweep direction to transform and add to the bounding box.</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>
/// Creates an affine transform from a rigid transform.
/// </summary>
/// <param name="rigid">Rigid transform to base the affine transform on.</param>
/// <param name="affine">Affine transform created from the rigid transform.</param>
public static void CreateFromRigidTransform(ref RigidTransform rigid, out AffineTransform affine)
{
affine.Translation = rigid.Position;
Matrix3X3.CreateFromQuaternion(ref rigid.Orientation, out affine.LinearTransform);
}
BepuEntity createFromMesh(Vector3 position, string mesh, float scale)
{
BepuEntity entity = new BepuEntity();
entity.modelName = mesh;
entity.LoadContent();
Vector3[] vertices;
int[] indices;
TriangleMesh.GetVerticesAndIndicesFromModel(entity.model, out vertices, out indices);
AffineTransform localTransform = new AffineTransform(new Vector3(scale, scale, scale), Quaternion.Identity, new Vector3(0, 0, 0));
MobileMesh mobileMesh = new MobileMesh(vertices, indices, localTransform, BEPUphysics.CollisionShapes.MobileMeshSolidity.Counterclockwise, 1);
entity.localTransform = Matrix.CreateScale(scale, scale, scale);
entity.body = mobileMesh;
entity.HasColor = true;
entity.diffuse = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
entity.body.Position = position;
space.Add(entity.body);
children.Add(entity);
return entity;
}
public static void Validate(this AffineTransform a)
{
a.LinearTransform.Validate();
a.Translation.Validate();
}
///<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>
/// 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>
///<param name="distributionInfo">Information computed about the shape during construction.</param>
public MobileMeshShape(Vector3[] vertices, int[] indices, AffineTransform localTransform, MobileMeshSolidity solidity, out ShapeDistributionInformation distributionInfo)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, localTransform);
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();
//ComputeBoundingHull();
}
/// <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;
}
///<summary>
/// Inverts an affine transform.
///</summary>
///<param name="transform">Transform to invert.</param>
/// <param name="inverse">Inverse of the transform.</param>
public static void Invert(ref AffineTransform transform, out AffineTransform inverse)
{
Matrix3X3.Invert(ref transform.LinearTransform, out inverse.LinearTransform);
Matrix3X3.Transform(ref transform.Translation, ref inverse.LinearTransform, out inverse.Translation);
Vector3.Negate(ref inverse.Translation, out inverse.Translation);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public StaticGroupDemo(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.
List<Collidable> 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();
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);
game.ModelDrawer.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;
TriangleMesh.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);
game.ModelDrawer.Add(mesh);
}
}
}
StaticGroup group = new StaticGroup(collidables);
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));
}
Box ground = new Box(new Vector3(0, -3f, 0), 200, 1, 200);
Space.Add(ground);
game.Camera.Position = new Vector3(0, 60, 90);
}
///<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>
/// Transforms a vector by an affine transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="transformed">Transformed position.</param>
public static void Transform(ref Vector3 position, ref AffineTransform transform, out Vector3 transformed)
{
Matrix3X3.Transform(ref position, ref transform.LinearTransform, out transformed);
Vector3.Add(ref transformed, ref transform.Translation, out transformed);
}
/// <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);
}
void CreateTerrain()
{
float peakValue = 0;
for (int x = 0; x < map.GetLength(0); x++)
for (int y = 0; y < map.GetLength(0); y++)
if (map[x, y] > peakValue) peakValue = map[x, y];
float half = (map.GetLength(0) * lateralScale) / 2;
AffineTransform transform = new AffineTransform(
new Vector3(lateralScale, verticalScale, -lateralScale),
Quaternion.Identity,
new Vector3(-half, 0, half));
Terrain = new Terrain(map, transform);
Sector.Redria.Space.Add(Terrain);
}
/// <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>
/// 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);
}
