/BEPUphysics/NarrowPhaseSystems/Pairs/InstancedMeshPairHandler.cs
C# | 216 lines | 146 code | 37 blank | 33 comment | 27 complexity | b886d66e21b7a0f3dfde93aa49197b54 MD5 | raw file
Possible License(s): CC-BY-SA-3.0, BSD-3-Clause, MPL-2.0-no-copyleft-exception, LGPL-2.1
- using System;
- using BEPUphysics.BroadPhaseEntries;
- using BEPUphysics.BroadPhaseEntries.MobileCollidables;
- using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK;
- using BEPUphysics.CollisionTests.Manifolds;
- using BEPUphysics.Constraints.Collision;
- using BEPUphysics.DataStructures;
- using BEPUutilities.DataStructures;
- using BEPUphysics.PositionUpdating;
- using BEPUphysics.Settings;
- using Microsoft.Xna.Framework;
- using BEPUutilities;
-
- namespace BEPUphysics.NarrowPhaseSystems.Pairs
- {
- ///<summary>
- /// Handles a instanced mesh-convex collision pair.
- ///</summary>
- public abstract class InstancedMeshPairHandler : StandardPairHandler
- {
- InstancedMesh instancedMesh;
- ConvexCollidable convex;
-
- NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint();
-
- public override Collidable CollidableA
- {
- get { return convex; }
- }
- public override Collidable CollidableB
- {
- get { return instancedMesh; }
- }
- public override Entities.Entity EntityA
- {
- get { return convex.entity; }
- }
- public override Entities.Entity EntityB
- {
- get { return null; }
- }
- /// <summary>
- /// Gets the contact constraint used by the pair handler.
- /// </summary>
- public override ContactManifoldConstraint ContactConstraint
- {
- get { return contactConstraint; }
- }
- /// <summary>
- /// Gets the contact manifold used by the pair handler.
- /// </summary>
- public override ContactManifold ContactManifold
- {
- get { return MeshManifold; }
- }
- protected abstract InstancedMeshContactManifold MeshManifold { get; }
-
- ///<summary>
- /// Initializes the pair handler.
- ///</summary>
- ///<param name="entryA">First entry in the pair.</param>
- ///<param name="entryB">Second entry in the pair.</param>
- public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
- {
-
- instancedMesh = entryA as InstancedMesh;
- convex = entryB as ConvexCollidable;
-
- if (instancedMesh == null || convex == null)
- {
- instancedMesh = entryB as InstancedMesh;
- convex = entryA as ConvexCollidable;
-
- if (instancedMesh == null || convex == null)
- throw new Exception("Inappropriate types used to initialize pair.");
- }
-
- //Contact normal goes from A to B.
- broadPhaseOverlap.entryA = convex;
- broadPhaseOverlap.entryB = instancedMesh;
-
- UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, instancedMesh.material);
-
-
- base.Initialize(entryA, entryB);
-
-
-
-
-
- }
-
-
- ///<summary>
- /// Cleans up the pair handler.
- ///</summary>
- public override void CleanUp()
- {
- base.CleanUp();
- instancedMesh = null;
- convex = null;
-
- }
-
-
-
- ///<summary>
- /// Updates the time of impact for the pair.
- ///</summary>
- ///<param name="requester">Collidable requesting the update.</param>
- ///<param name="dt">Timestep duration.</param>
- public override void UpdateTimeOfImpact(Collidable requester, float dt)
- {
- //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us.
- if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
- {
- //TODO: This system could be made more robust by using a similar region-based rejection of edges.
- //CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
-
- //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
- Vector3 velocity;
- Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
- float velocitySquared = velocity.LengthSquared();
-
- var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
- timeOfImpact = 1;
- if (minimumRadius * minimumRadius < velocitySquared)
- {
- var triangle = PhysicsResources.GetTriangle();
- triangle.collisionMargin = 0;
- //Spherecast against all triangles to find the earliest time.
- for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++)
- {
- MeshBoundingBoxTreeData data = instancedMesh.Shape.TriangleMesh.Data;
- int triangleIndex = MeshManifold.overlappedTriangles.Elements[i];
- data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
- AffineTransform.Transform(ref triangle.vA, ref instancedMesh.worldTransform, out triangle.vA);
- AffineTransform.Transform(ref triangle.vB, ref instancedMesh.worldTransform, out triangle.vB);
- AffineTransform.Transform(ref triangle.vC, ref instancedMesh.worldTransform, out triangle.vC);
- //Put the triangle into 'localish' space of the convex.
- Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
- Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
- Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
-
- RayHit rayHit;
- if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
- rayHit.T > Toolbox.BigEpsilon)
- {
-
- if (instancedMesh.sidedness != TriangleSidedness.DoubleSided)
- {
- Vector3 AB, AC;
- Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
- Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
- Vector3 normal;
- Vector3.Cross(ref AB, ref AC, out normal);
- float dot;
- Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
- //Only perform sweep if the object is in danger of hitting the object.
- //Triangles can be one sided, so check the impact normal against the triangle normal.
- if (instancedMesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
- instancedMesh.sidedness == TriangleSidedness.Clockwise && dot > 0)
- {
- timeOfImpact = rayHit.T;
- }
- }
- else
- {
- timeOfImpact = rayHit.T;
- }
- }
- }
- PhysicsResources.GiveBack(triangle);
- }
-
-
-
- }
-
- }
-
-
- protected internal override void GetContactInformation(int index, out ContactInformation info)
- {
- info.Contact = MeshManifold.contacts.Elements[index];
- //Find the contact's normal and friction forces.
- info.FrictionImpulse = 0;
- info.NormalImpulse = 0;
- for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++)
- {
- if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact)
- {
- info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse;
- info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse;
- break;
- }
- }
-
- //Compute relative velocity
- if (convex.entity != null)
- {
- Vector3 velocity;
- Vector3.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity);
- Vector3.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity);
- Vector3.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity);
- }
- else
- info.RelativeVelocity = new Vector3();
-
-
- info.Pair = this;
- }
-
- }
-
- }