Unfortunately, something is going wrong somewhere. Specifically, the box only correctly collides when the box isn't rotated. Otherwise, it may not collide, or it may collide at various wrong locations. Naturally, this is somewhat less than ideal. If you need screenshots, I could take some.
So: The actual box collision code:
private static bool Intersection( Vector3 minimum, Vector3 maximum, Vector3 previousPositionA, Vector3 currentPositionA, float radiusB, Vector3 previousPositionB, Vector3 currentPositionB, out CollisionData collisionData, float maxTime ) { // Yoinked and adapted. Note the clever conversion to a vector distance. float sideDistance; Vector3 squaredDistance = Vector3.Zero; for (int i = 0; i < 3; ++i) { if (previousPositionA < minimum) { sideDistance = previousPositionA - minimum; squaredDistance = sideDistance * sideDistance; } else if (previousPositionA > maximum) { sideDistance = previousPositionA - maximum; squaredDistance = sideDistance * sideDistance; } } // My own code. Probably buggy. if (squaredDistance.X + squaredDistance.Y + squaredDistance.Z <= radiusB * radiusB) { Vector3 distance = squaredDistance; distance = SGLMath.SafeSqrt(distance); Vector3 velocityA = currentPositionA - previousPositionA; Vector3 velocityB = currentPositionB - previousPositionB; Vector3 relativeVelocity = velocityB - velocityA; Vector3 timeVector = new Vector3( distance.X / relativeVelocity.X, distance.Y / relativeVelocity.Y, distance.Z / relativeVelocity.Z ); timeVector = SGLMath.ZeroNaNOrInfinity(timeVector); float t1 = timeVector.Length(); Vector3 collisionPoint = (previousPositionA + distance) + (Vector3.Normalize(distance) * radiusB); // Vector3 collisionPoint = (previousPositionB - distance) - (Vector3.Normalize(distance) * radiusB); Vector3 collisionPositionA = previousPositionA + (velocityA * t1); collisionData = CollisionData.GenerateCollisonData( ref previousPositionA, ref previousPositionB, ref collisionPositionA, ref collisionPoint, ref velocityA, ref velocityB, ref relativeVelocity, t1 ); return true; } collisionData = new CollisionData(false); return false; }
And the code that is supposed to rotate all this into the boxes' reference frame, then back again, so everyone gets the correct results:
// Also my code; also probably buggy. Matrix reverseOrientation = Matrix.Invert(orientationA); Vector3 newPreviousPositionB = previousPositionA + Vector3.TransformCoordinate(previousPositionB - previousPositionA, reverseOrientation); Vector3 newCurrentPositionB = currentPositionA + Vector3.TransformCoordinate(currentPositionB - currentPositionA, reverseOrientation); bool found = Intersection( minimum, maximum, previousPositionA, currentPositionA, radiusB, newPreviousPositionB, newCurrentPositionB, out collisionData, maxTime ); collisionData.Normal = Vector3.TransformCoordinate(collisionData.Normal, orientationA); collisionData.Point = collisionData.PositionA + Vector3.TransformCoordinate((collisionData.Point - collisionData.PositionA), orientationA); collisionData.PositionA = previousPositionA + Vector3.TransformCoordinate((collisionData.PositionA - previousPositionA), orientationA); collisionData.PositionB = previousPositionB + Vector3.TransformCoordinate((collisionData.PositionB - previousPositionB), orientationA); return found;
Please and thank you.