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; }