public static GJKResult GJK(Vector3 aPos, List <Vector3> aVertices, Vector3 bPos, List <Vector3> bVertices, bool twoD = false)
{
var result = new GJKResult
{
Simplex = new List <Vector3>(),
AVertices = aVertices,
BVertices = bVertices
};
var direction = aPos - bPos;
var support = Support(aVertices, bVertices, direction);
result.Simplex.Add(support);
direction = support * -1;
while (true)
{
support = Support(aVertices, bVertices, direction);
if (Vector3.Dot(support, direction) <= 0)
{
return(new GJKResult());
}
result.Simplex.Add(support);
var(collision, newDirection) = NextSimplex(result.Simplex, direction, twoD);
if (collision)
{
result.Collision = true;
return(result);
}
direction = newDirection;
}
}
private (Vector3 position, Vector3 orientation) InterpolateCollision(IEntity entity, IEntity other,
Vector3 position, Vector3 otherPosition, Vector3 orientation, GJKResult result)
{
var ePhysicsComponent = entity.GetComponent <PhysicsComponent>();
// Don't try to move the position if the entity is fixed
if (ePhysicsComponent.Fixed)
{
return(position, orientation);
}
var oPhysicsComponent = other.GetComponent <PhysicsComponent>();
var interpolatedPosition = new Vector3(position.X, position.Y, position.Z);
var interpolatedOrientation = new Vector3(orientation.X, orientation.Y, orientation.Z);
// Simulate fixed collisions
if (oPhysicsComponent.Fixed && oPhysicsComponent.Solid)
{
var epaResult = Algorithms.EPA(result);
// Fix orientation based on the penetration vector
var dOrientation = new Vector3(InterpolateOrientation(interpolatedOrientation.X, epaResult.Y, epaResult.Z),
InterpolateOrientation(interpolatedOrientation.Y, epaResult.Z, epaResult.X),
InterpolateOrientation(interpolatedOrientation.Z, epaResult.X, epaResult.Y));
interpolatedOrientation += dOrientation;
ePhysicsComponent.AngularVelocity = new Vector3(dOrientation.X != 0 ? 0 : ePhysicsComponent.AngularVelocity.X,
dOrientation.Y != 0 ? 0 : ePhysicsComponent.AngularVelocity.Y,
dOrientation.Z != 0 ? 0 : ePhysicsComponent.AngularVelocity.Z);
// Correct the position based on the penetration vector
var correctedResult = new Vector3(
CorrectEPAResult(epaResult.X, epaResult.Y, epaResult.Z, Gravity.Y, Gravity.Z),
CorrectEPAResult(epaResult.Y, epaResult.X, epaResult.Z, Gravity.X, Gravity.Z),
CorrectEPAResult(epaResult.Z, epaResult.X, epaResult.Y, Gravity.X, Gravity.Y));
interpolatedPosition -= correctedResult;
ePhysicsComponent.Velocity = new Vector3(InterpolateVelocity(ePhysicsComponent.Velocity.X, correctedResult.X, Gravity.X),
InterpolateVelocity(ePhysicsComponent.Velocity.Y, correctedResult.Y, Gravity.Y),
InterpolateVelocity(ePhysicsComponent.Velocity.Z, correctedResult.Z, Gravity.Z));
}
// TODO Simulate elastics collisions
return(interpolatedPosition, interpolatedOrientation);
}
