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); }