private void ComputeMovement(ref Vector3 position, ref Vector3 velocity, Vector3 radius, int recursive)
{
if (recursive > 8 || velocity == Vector3.Zero)
return;
// Ellipsioid space conversion
position = position / radius;
velocity = velocity / radius;
float t = 1;
Vector3 collisionPoint = Vector3.Zero;
Vector3 planeNormal = Vector3.Zero;
// Compute trianle collision detection (t = factor at wich collision occurs)
CalculateCollision(position, velocity, radius, out t, out collisionPoint, out planeNormal);
Vector3 newPosition = position + t * velocity;
if (t < 1)
{
// Create sliding plane
Vector3 collisionNormal = Vector3.Normalize(position - collisionPoint);
Vector3 destinationPosition = position + velocity;
Plane slidingPlane = GeometryMath.PlaneFromPointNormal(collisionPoint, collisionNormal);
Vector3 newVelocity = (destinationPosition - slidingPlane.SignedDistance(destinationPosition) * slidingPlane.Normal) - collisionPoint;
// Check for steep hill
float normalDotVelocity = Vector3.Dot(collisionNormal, velocity);
if (Math.Abs(planeNormal.Y) < 0.707f && normalDotVelocity < 0)
newVelocity.Y = 0;
// Compute recursive function
position = newPosition * radius;
velocity = newVelocity * radius;
ComputeMovement(ref position, ref velocity, radius, recursive + 1);
return;
}
position = newPosition * radius;
velocity = velocity * t * radius;
}