private bool DoDeepContact(out ContactData contact) { if (previousState == CollisionState.Separated ) //If it was shallow before, then its closest points will be used to find the normal. { //It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space! //The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine. if (collidableA.entity != null && collidableB.entity != null) { Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection); } else { localDirection = localSeparatingAxis; } if (localDirection.LengthSquared() < Toolbox.Epsilon) { localDirection = Vector3.Up; } } if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact)) { if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) { state = CollisionState.ShallowContact; } return(true); } //This is rare, but could happen. state = CollisionState.Separated; return(false); }
private bool DoDeepContact(out ContactData contact) { #region Informed search if (previousState == CollisionState.Separated) //If it was shallow before, then its closest points will be used to find the normal. { //It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space! //Vector3 velocityA; //Vector3.Cross(ref contact.Position, ref collidableA.entity.angularVelocity, out velocityA); //Vector3.Add(ref velocityA, ref collidableA.entity.linearVelocity, out velocityA); //Vector3 velocityB; //Vector3.Subtract(ref contact.Position, ref localTransformB.Position, out velocityB); //Vector3.Cross(ref velocityB, ref collidableB.entity.angularVelocity, out velocityB); //Vector3.Add(ref velocityB, ref collidableB.entity.linearVelocity, out velocityB); ////The velocity is negated because the direction so point backwards along the velocity. //Vector3.Subtract(ref velocityA, ref velocityB, out localDirection); //The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine. if (collidableA.entity != null && collidableB.entity != null) { Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection); } else { localDirection = localSeparatingAxis; } if (localDirection.LengthSquared() < Toolbox.Epsilon) { localDirection = Vector3.Up; } } if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact)) { if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) { state = CollisionState.ShallowContact; } return(true); } //This is rare, but could happen. state = CollisionState.Separated; return(false); //if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position)) //{ // //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity. // Vector3 rayCastDirection; // float lengthSquared = localDirection.LengthSquared(); // if (lengthSquared > Toolbox.Epsilon) // { // Vector3.Divide(ref localDirection, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2; // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal); // } // else // { // contact.PenetrationDepth = float.MaxValue; // contact.Normal = Toolbox.UpVector; // } // //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity. // //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization). // Vector3 normalCandidate; // float depthCandidate; // lengthSquared = localTransformB.Position.LengthSquared(); // if (lengthSquared > Toolbox.Epsilon) // { // Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection); // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate); // if (depthCandidate < contact.PenetrationDepth) // { // contact.Normal = normalCandidate; // } // } // //Correct the penetration depth. // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection); // ////The local casting can optionally continue. Eventually, it will converge to the local minimum. // //while (true) // //{ // // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out depthCandidate, out normalCandidate); // // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon) // // break; // // contact.PenetrationDepth = depthCandidate; // // contact.Normal = normalCandidate; // //} // contact.Id = -1; // //we're still in local space! transform it all back. // Matrix3X3 orientation; // Matrix3X3.CreateFromQuaternion(ref collidableA.worldTransform.Orientation, out orientation); // Matrix3X3.Transform(ref contact.Normal, ref orientation, out contact.Normal); // //Vector3.Negate(ref contact.Normal, out contact.Normal); // Matrix3X3.Transform(ref contact.Position, ref orientation, out contact.Position); // Vector3.Add(ref contact.Position, ref collidableA.worldTransform.Position, out contact.Position); // if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) // state = CollisionState.ShallowContact; // return true; //} ////This is rare, but could happen. //state = CollisionState.Separated; //contact = new ContactData(); //return false; #endregion #region Testing //RigidTransform localTransformB; //MinkowskiToolbox.GetLocalTransform(ref collidableA.worldTransform, ref collidableB.worldTransform, out localTransformB); //contact.Id = -1; //if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position)) //{ // Vector3 rayCastDirection = localTransformB.Position; // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal); // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection); // RigidTransform.Transform(ref contact.Position, ref collidableA.worldTransform, out contact.Position); // Vector3.Transform(ref contact.Normal, ref collidableA.worldTransform.Orientation, out contact.Normal); // return true; //} //contact.Normal = new Vector3(); //contact.PenetrationDepth = 0; //return false; #endregion #region v0.15.2 and before //if (MPRToolbox.AreObjectsColliding(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, out contact)) //{ // if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) // state = CollisionState.ShallowContact; //If it's emerged from the deep contact, we can go back to using the preferred GJK method. // return true; //} ////This is rare, but could happen. //state = CollisionState.Separated; //return false; #endregion }