/// <summary>
/// Currently each object falls under gravity and interacts with the flat floor using a basic collision algorithm.
/// The collisions consist of a reaction force which pushes away from the floor, and a friction force which dampens
/// motion. The collision forces are summed over each vertex which intersects with the floor.
/// You could add other interactions here - elastic springs, inter-object collision, explosions, push/pull forces...
/// </summary>
private void UpdatePhysics(float timeDelta)
{
// Plank
Vector3 plankCollisionForce = Vector3.Zero;
Vector3 plankCollisionTorque = Vector3.Zero;
foreach (Vector3 vertexPosition in _plankRigidBody.BoundingVertices)
{
Vector3 vertexLocalPosition = Vector3.Transform(vertexPosition, _plankRigidBody.State.Orientation);
Vector3 vertexWorldPosition = _plankRigidBody.State.Position + vertexLocalPosition;
float penetrationDepth = CollisionBias - vertexWorldPosition.Y;
if (penetrationDepth >= 0)
{
Vector3 collisionNormal = Vector3.UnitY;
Vector3 vertexVelocity = _plankRigidBody.State.Velocity + Vector3.Cross(_plankRigidBody.State.AngularVelocity, vertexLocalPosition);
Vector3 vertexCollisionForce = _plankRigidBody.Mass * (ReactionForceMultiplier * collisionNormal * penetrationDepth - FrictionForceMultiplier * vertexVelocity);
plankCollisionForce += vertexCollisionForce;
plankCollisionTorque += Vector3.Cross(vertexLocalPosition, vertexCollisionForce);
}
}
_plankRigidBody.ApplyPhysics(_plankRigidBody.Mass * _gravity + plankCollisionForce, plankCollisionTorque, timeDelta);
// Cube
Vector3 cubeCollisionForce = Vector3.Zero;
Vector3 cubeCollisionTorque = Vector3.Zero;
foreach (Vector3 vertexPosition in _cubeRigidBody.BoundingVertices)
{
Vector3 vertexLocalPosition = Vector3.Transform(vertexPosition, _cubeRigidBody.State.Orientation);
Vector3 vertexWorldPosition = _cubeRigidBody.State.Position + vertexLocalPosition;
float penetrationDepth = CollisionBias - vertexWorldPosition.Y;
if (penetrationDepth >= 0)
{
Vector3 collisionNormal = Vector3.UnitY;
Vector3 vertexVelocity = _cubeRigidBody.State.Velocity + Vector3.Cross(_cubeRigidBody.State.AngularVelocity, vertexLocalPosition);
Vector3 vertexCollisionForce = ReactionForceMultiplier * collisionNormal * penetrationDepth - FrictionForceMultiplier * vertexVelocity;
cubeCollisionForce += vertexCollisionForce;
cubeCollisionTorque += Vector3.Cross(vertexLocalPosition, vertexCollisionForce);
}
}
_cubeRigidBody.ApplyPhysics(_gravity + cubeCollisionForce, cubeCollisionTorque, timeDelta);
}