///<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. System.Numerics.Vector3 velocity; Vector3Ex.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 = PhysicsThreadResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { mesh.Shape.TriangleMeshData.GetTriangle(MeshManifold.overlappedTriangles.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3Ex.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3Ex.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3Ex.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 (mesh.sidedness != TriangleSidedness.DoubleSided) { System.Numerics.Vector3 AB, AC; Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out AC); System.Numerics.Vector3 normal; Vector3Ex.Cross(ref AB, ref AC, out normal); float dot; Vector3Ex.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 (mesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 || mesh.sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsThreadResources.GiveBack(triangle); } } }
/// <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 = PhysicsThreadResources.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; PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(hit.T != float.MaxValue); } PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(false); }
/// <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 Vector3f sweep, out RayHit hit) { hit = new RayHit(); BoundingBox localSpaceBoundingBox; castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out localSpaceBoundingBox); var tri = PhysicsThreadResources.GetTriangle(); var hitElements = new QuickList <int>(BufferPools <int> .Thread); if (Shape.GetOverlaps(localSpaceBoundingBox, ref hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.GetTriangle(hitElements.Elements[i], ref worldTransform, out tri.vA, out tri.vB, out tri.vC); Vector3f center; Vector3f.Add(ref tri.vA, ref tri.vB, out center); Vector3f.Add(ref center, ref tri.vC, out center); Vector3f.Multiply(ref center, 1f / 3f, out center); Vector3f.Subtract(ref tri.vA, ref center, out tri.vA); Vector3f.Subtract(ref tri.vB, ref center, out tri.vB); Vector3f.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; PhysicsThreadResources.GiveBack(tri); hitElements.Dispose(); return(hit.T != float.MaxValue); } PhysicsThreadResources.GiveBack(tri); hitElements.Dispose(); return(false); }
///<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) { var overlap = BroadPhaseOverlap; var meshMode = mobileMesh.entity == null ? PositionUpdateMode.Discrete : mobileMesh.entity.PositionUpdateMode; var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode; if ( (mobileMesh.IsActive || convex.IsActive) && //At least one has to be active. ( ( convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A. meshMode == PositionUpdateMode.Continuous && overlap.entryA == requester ) || ( convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it. meshMode == PositionUpdateMode.Continuous ) ) ) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; if (convexMode == PositionUpdateMode.Discrete) { //Convex is static for the purposes of CCD. Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity); } else if (meshMode == PositionUpdateMode.Discrete) { //Mesh is static for the purposes of CCD. velocity = convex.entity.linearVelocity; } else { //Both objects can move. Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity); } Vector3.Multiply(ref velocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { TriangleSidedness sidedness = mobileMesh.Shape.Sidedness; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation); var triangle = PhysicsThreadResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data; int triangleIndex = MeshManifold.overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC); Matrix3x3.Transform(ref triangle.vA, ref orientation, out triangle.vA); Matrix3x3.Transform(ref triangle.vB, ref orientation, out triangle.vB); Matrix3x3.Transform(ref triangle.vC, ref orientation, out triangle.vC); Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA); Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB); Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (sidedness != TriangleSidedness.DoubleSided) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); float dot; Vector3.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 || sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsThreadResources.GiveBack(triangle); } } }
///<summary> /// Updates the manifold. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //First, refresh all existing contacts. This is an incremental manifold. var transform = MeshTransform; ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove); RemoveQueuedContacts(); CleanUpOverlappingTriangles(); //Get all the overlapped triangle indices. //Note that the collection of triangles is left up to the child implementation. //We're basically treating the child class like an indexable collection. //A little gross to have it organized this way instead of an explicit collection to separate the logic up. Would be nice to improve someday! int triangleCount = FindOverlappingTriangles(dt); //Just use 32 elements for all the lists and sets in this system. const int bufferPoolSizePower = 5; BoundarySets boundarySets; if (UseImprovedBoundaryHandling) { boundarySets = new BoundarySets(bufferPoolSizePower); } else { boundarySets = new BoundarySets(); } var candidatesToAdd = new QuickList <ContactData>(BufferPools <ContactData> .Thread, bufferPoolSizePower); //A single triangle shape will be reused for all operations. It's pulled from a thread local pool to avoid holding a TriangleShape around for every single contact manifold or pair tester. var localTriangleShape = PhysicsThreadResources.GetTriangle(); //Precompute the transform to take triangles from their native local space to the convex's local space. RigidTransform inverseConvexWorldTransform; RigidTransform.Invert(ref convex.worldTransform, out inverseConvexWorldTransform); AffineTransform convexInverseWorldTransform; AffineTransform.CreateFromRigidTransform(ref inverseConvexWorldTransform, out convexInverseWorldTransform); AffineTransform fromMeshLocalToConvexLocal; PrecomputeTriangleTransform(ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal); //Grab the convex's local space bounding box up front. This will be used for a secondary pruning step. BoundingBox convexLocalBoundingBox; convex.Shape.GetBoundingBox(ref Toolbox.RigidIdentity, out convexLocalBoundingBox); Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation); var guaranteedContacts = 0; for (int i = 0; i < triangleCount; i++) { //Initialize the local triangle. TriangleIndices indices; if (ConfigureLocalTriangle(i, localTriangleShape, out indices)) { //Put the triangle into the local space of the convex. AffineTransform.Transform(ref localTriangleShape.vA, ref fromMeshLocalToConvexLocal, out localTriangleShape.vA); AffineTransform.Transform(ref localTriangleShape.vB, ref fromMeshLocalToConvexLocal, out localTriangleShape.vB); AffineTransform.Transform(ref localTriangleShape.vC, ref fromMeshLocalToConvexLocal, out localTriangleShape.vC); //Do one last AABB test between the convex and triangle in the convex's local space. //This can prune out a lot of triangles when dealing with larger objects, and it's pretty cheap to do. BoundingBox triangleBoundingBox; Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleBoundingBox); bool intersecting; triangleBoundingBox.Intersects(ref convexLocalBoundingBox, out intersecting); if (!intersecting) { continue; } //Find a pairtester for the triangle. TrianglePairTester pairTester; if (!activePairTesters.TryGetValue(indices, out pairTester)) { pairTester = GetTester(); pairTester.Initialize(convex.Shape); activePairTesters.Add(indices, pairTester); } pairTester.Updated = true; //Now, generate a contact between the two shapes. TinyStructList <ContactData> contactList; if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList)) { for (int j = 0; j < contactList.Count; j++) { ContactData contact; contactList.Get(j, out contact); if (UseImprovedBoundaryHandling) { if (AnalyzeCandidate(ref indices, localTriangleShape, pairTester, ref contact, ref boundarySets)) { //This is let through if there's a face contact. Face contacts cannot be blocked. guaranteedContacts++; AddLocalContact(ref contact, ref orientation, ref candidatesToAdd); } } else { AddLocalContact(ref contact, ref orientation, ref candidatesToAdd); } } } //Get the voronoi region from the contact candidate generation. Possibly just recalculate, since most of the systems don't calculate it. //Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region. For face contacts, don't bother- just add it if unique. //For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices. //For A, B, or C, add the index of the vertex to the blockedVertexRegions set. //If the edge/vertex is already present in the set, then DO NOT add the contact. //When adding a contact, add ALL other voronoi regions to the blocked sets. } } if (UseImprovedBoundaryHandling) { //If there were no face contacts that absolutely must be included, we may get into a very rare situation //where absolutely no contacts get created. For example, a sphere falling directly on top of a vertex in a flat terrain. //It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices). //In some cases, the contacts will be ignored instead of corrected (e.g. spheres). //To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires. //Sure, it might not be necessary under normal circumstances, but it's a better option than having no contacts. //TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge, //and an edge only restricts the far vertex and other two edges. This introduces an occasional bump though... //It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles. //For this state to continue beyond a brief instant generally requires the object be orientation locked and slender. //However, some characters fit this description, so it can't be ignored! //Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only). //However, an object stuck between multiple triangles is not in a stable state. In the edge case, the object gets shoved to one side //as one contact 'wins' the solver war. That's not enough to escape, unfortunately. //The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability. //So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :) //This isn't a completely free operation, but it's guarded behind pretty rare conditions. //Essentially, we will check to see if there's just edge contacts fighting against each other. //If they are, then we will correct any stuck-contributing normals to the triangle normal. if (boundarySets.VertexContacts.Count == 0 && guaranteedContacts == 0 && boundarySets.EdgeContacts.Count > 1) { //There are only edge contacts, check to see if: //all normals are coplanar, and //at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner). bool allNormalsInSamePlane = true; bool atLeastOneNormalAgainst = false; var firstNormal = boundarySets.EdgeContacts.Elements[0].ContactData.Normal; boundarySets.EdgeContacts.Elements[0].CorrectedNormal.Normalize(); float dot; Vector3Ex.Dot(ref firstNormal, ref boundarySets.EdgeContacts.Elements[0].CorrectedNormal, out dot); if (Math.Abs(dot) > .01f) { //Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity. allNormalsInSamePlane = false; } else { //TODO: Note that we're only checking the new edge contacts, not the existing contacts. //It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity //we'll ignore that possibility for now. for (int i = 1; i < boundarySets.EdgeContacts.Count; i++) { Vector3Ex.Dot(ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot); if (dot < 0) { atLeastOneNormalAgainst = true; } //Check to see if the normal is outside the plane. Vector3Ex.Dot(ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, ref boundarySets.EdgeContacts.Elements[0].CorrectedNormal, out dot); if (Math.Abs(dot) > .01f) { //We are not stuck! allNormalsInSamePlane = false; break; } } } if (allNormalsInSamePlane && atLeastOneNormalAgainst) { //Uh oh! all the normals are parallel... The object is probably in a weird situation. //Let's correct the normals! //Already normalized the first contact above. //We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already. boundarySets.EdgeContacts.Elements[0].ContactData.Normal = boundarySets.EdgeContacts.Elements[0].CorrectedNormal; boundarySets.EdgeContacts.Elements[0].ShouldCorrect = true; for (int i = 1; i < boundarySets.EdgeContacts.Count; i++) { //Must normalize the corrected normal before using it. boundarySets.EdgeContacts.Elements[i].CorrectedNormal.Normalize(); Vector3Ex.Dot(ref boundarySets.EdgeContacts.Elements[i].CorrectedNormal, ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, out dot); if (dot < .01) { //Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness! //If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal. //Make them the same thing :) boundarySets.EdgeContacts.Elements[i].ContactData.Normal = boundarySets.EdgeContacts.Elements[i].CorrectedNormal; boundarySets.EdgeContacts.Elements[i].ShouldCorrect = true; //Note that the penetration depth is NOT corrected. The contact's depth no longer represents the true depth. //However, we only need to have some penetration depth to get the object to escape the rut. //Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction. //If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system //would have picked a different depth, as it finds a reasonable approximation of the minimum penetration! //As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it). } } } } for (int i = 0; i < boundarySets.EdgeContacts.Count; i++) { //Only correct if it's allowed AND it's blocked. //If it's not blocked, the contact being created is necessary! //The normal generated by the triangle-convex tester is already known not to //violate the triangle sidedness. if (!boundarySets.BlockedEdgeRegions.Contains(boundarySets.EdgeContacts.Elements[i].Edge)) { //If it's not blocked, use the contact as-is without correcting it. AddLocalContact(ref boundarySets.EdgeContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd); } else if (boundarySets.EdgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0) { //If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that //it will not interfere (and cause a bump or something). float dot; boundarySets.EdgeContacts.Elements[i].CorrectedNormal.Normalize(); Vector3Ex.Dot(ref boundarySets.EdgeContacts.Elements[i].CorrectedNormal, ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, out dot); boundarySets.EdgeContacts.Elements[i].ContactData.Normal = boundarySets.EdgeContacts.Elements[i].CorrectedNormal; boundarySets.EdgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth. AddLocalContact(ref boundarySets.EdgeContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd); } //If it's blocked AND it doesn't allow correction, ignore its existence. } for (int i = 0; i < boundarySets.VertexContacts.Count; i++) { if (!boundarySets.BlockedVertexRegions.Contains(boundarySets.VertexContacts.Elements[i].Vertex)) { //If it's not blocked, use the contact as-is without correcting it. AddLocalContact(ref boundarySets.VertexContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd); } else if (boundarySets.VertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0) { //If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that //it will not interfere (and cause a bump or something). float dot; boundarySets.VertexContacts.Elements[i].CorrectedNormal.Normalize(); Vector3Ex.Dot(ref boundarySets.VertexContacts.Elements[i].CorrectedNormal, ref boundarySets.VertexContacts.Elements[i].ContactData.Normal, out dot); boundarySets.VertexContacts.Elements[i].ContactData.Normal = boundarySets.VertexContacts.Elements[i].CorrectedNormal; boundarySets.VertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth. AddLocalContact(ref boundarySets.VertexContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd); } //If it's blocked AND it doesn't allow correction, ignore its existence. } boundarySets.Dispose(); } //Remove stale pair testers. for (int i = activePairTesters.Count - 1; i >= 0; --i) { var tester = activePairTesters.Values[i]; if (!tester.Updated) { tester.CleanUp(); GiveBackTester(tester); activePairTesters.FastRemove(activePairTesters.Keys[i]); } else { tester.Updated = false; } } //Some child types will want to do some extra post processing on the manifold. ProcessCandidates(ref candidatesToAdd); //Check if adding the new contacts would overflow the manifold. if (contacts.Count + candidatesToAdd.Count > 4) { //Adding all the contacts would overflow the manifold. Reduce to the best subset. var reducedCandidates = new QuickList <ContactData>(BufferPools <ContactData> .Thread, bufferPoolSizePower); ContactReducer.ReduceContacts(contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates); RemoveQueuedContacts(); for (int i = reducedCandidates.Count - 1; i >= 0; i--) { Add(ref reducedCandidates.Elements[i]); reducedCandidates.RemoveAt(i); } reducedCandidates.Dispose(); } else if (candidatesToAdd.Count > 0) { //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else. for (int i = 0; i < candidatesToAdd.Count; i++) { Add(ref candidatesToAdd.Elements[i]); } } PhysicsThreadResources.GiveBack(localTriangleShape); candidatesToAdd.Dispose(); }
/// <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 = F64.C0 }; 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 = PhysicsThreadResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = Fix64.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, F64.OneThird, 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(); Fix64 radius = tri.vB.LengthSquared(); if (tri.MaximumRadius < radius) { tri.MaximumRadius = radius; } radius = tri.vC.LengthSquared(); if (tri.MaximumRadius < radius) { tri.MaximumRadius = radius; } tri.MaximumRadius = Fix64.Sqrt(tri.MaximumRadius); tri.collisionMargin = F64.C0; 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 = F64.C0; PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(hit.T != Fix64.MaxValue); } PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(false); }