// =====
#region Update
// Update
public void Update()
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0}", this.Name));
#endif
// Skip if paused
if (this.IsPaused)
{
return;
}
/* // Test spool
* if (Globals.TestBool)
* {
* RigidBody rb = (RigidBody)Globals.Scene.Bodies[0];
* ConvexPolygon cp = (ConvexPolygon)Globals.Scene.Bodies[0].Geometry;
* Vector2 p = cp.Vertices[2];
* Vector2 v = rb.Velocity;
* float w = rb.AngularVelocity;
* Vector2 vp = cp.GetVelocityAtPoint(p, v, w);
* Globals.TextFile.Write(String.Format("{0}\t{1}\t{2}", Globals.MathHelper.ToDegrees(cp.Angle), vp.X, vp.Y));
* } */
// Reset elapsedSceneTime
this.elapsedSceneTime = 0.0f;
// Update targetTotalTime
this.targetTotalTime += Globals.Clock.ElapsedGameTime;
// Declare minCollisionTime
float minCollisionTime;
// Initialize minCollisions
List <CollisionResult> minCollisions = new List <CollisionResult>();
// Initialize restingContacts
List <CollisionResult> restingContacts = new List <CollisionResult>();
// Reset substep count
this.Substep = 0;
// Initialize substep stopping criteria as false
bool stop = false;
// Begin simulation loop
while (!stop)
{
// Increment substep count
this.Substep++;
// Initialize minCollisionTime as targetElapsedTime
minCollisionTime = this.TargetElapsedTime;
// Clear contact manager
this.ContactManager.Clear();
// Clear minCollisions
minCollisions.Clear();
// Clear restingContacts
restingContacts.Clear();
#region [1] Find earliest time of impact
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("this.TotalSceneTime = {0}, this.Substep = {1}", this.TotalSceneTime, this.Substep));
Log.Write(String.Format("Now finding earliest time of impact"));
#endif
// Loop through all bodies in scene
for (int i = 0; i < this.Bodies.Count; i++)
{
// Loop through all other bodies in scene
for (int j = i + 1; j < this.Bodies.Count; j++)
{
// Perform a collision check
CollisionResult collisionResult = new CollisionResult(this.Bodies[i], this.Bodies[j], minCollisionTime);
// Check if this is a resting contact
if (collisionResult.IsResting)
{
// If so, create contact
// Contact contact = new Contact(collisionResult);
// Add to manager
// this.ContactManager.Add(contact);
// If so, add to restingContacts
restingContacts.Add(collisionResult);
}
// Update minCollisionTime
else if (collisionResult.Time < minCollisionTime)
{
#region Case 1: Strictly first
// Reset minCollision list
minCollisions.Clear();
minCollisions.Add(collisionResult);
// Update minCollisionTime
minCollisionTime = collisionResult.Time;
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (this.Name == Log.Subject1 && (Log.Subject2 == null || this.Bodies[j].Name == Log.Subject2)))
{
Log.Write(String.Format("This collision occurs strictly first with t = {0}", collisionResult.Time));
}
#endif
#endregion
}
else if (collisionResult.Time == minCollisionTime)
{
#region Case 2: Strictly first
// Add to minCollision list
minCollisions.Add(collisionResult);
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (this.Name == Log.Subject1 && (Log.Subject2 == null || this.Bodies[j].Name == Log.Subject2)))
{
Log.Write(String.Format("This collision is tied for first with t = {0}", collisionResult.Time));
}
#endif
#endregion
}
else
{
#region Case 3: Not first
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (this.Name == Log.Subject1 && (Log.Subject2 == null || this.Bodies[j].Name == Log.Subject2)))
{
Log.Write(String.Format("This collision does not occur first with t = {0} > {1}", collisionResult.Time, minCollisionTime));
}
#endif
#endregion
}
}
// Clear sweep object
this.Bodies[i].Sweep.Clear();
}
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("The earliest time of impact is {0}", minCollisionTime));
#endif
#endregion
#region [2] Perform a time step
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("Now performing a time step of minCollisionTime = {0} seconds", minCollisionTime));
#endif
// Loop through all bodies in scene
for (int i = 0; i < this.Bodies.Count; i++)
{
// Move body
this.Bodies[i].Move(minCollisionTime);
}
#endregion
#region [3] Find contact points
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("minCollisions.Count = {0}", minCollisions.Count));
Log.Write(String.Format("restingContacts.Count = {0}", restingContacts.Count));
Log.Write(String.Format("Now finding collision contact points"));
#endif
// Loop through minCollisions
for (int i = 0; i < minCollisions.Count; i++)
{
// Get contact
Contact contact = new Contact(minCollisions[i]);
// Add to contact manager
this.ContactManager.AddContact(contact);
}
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("Now finding resting contact points"));
#endif
// Loop through restingContacts
for (int i = 0; i < restingContacts.Count; i++)
{
// Get contact
Contact contact = new Contact(restingContacts[i]);
// Add to contact manager
this.ContactManager.AddContact(contact);
}
#endregion
#region [4] Update velocity due to external forces
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("Now updating velocity due to external forces"));
#endif
// Loop through all bodies in scene
for (int i = 0; i < this.Bodies.Count; i++)
{
// Point to body
RigidBody body = (RigidBody)this.Bodies[i];
// Add field impulses to queue
body.GetFieldImpulses(minCollisionTime);
// Process and drain applied impulse queue
body.FeelImpulses();
}
#endregion
#region [5] Resolve all contacts
// Physics logging
#if IS_LOGGING_PHYSICS
Log.Write(String.Format("Now resolving contacts"));
#endif
// Resolve all contacts
this.ContactManager.Resolve();
// Loop through all bodies in scene
for (int i = 0; i < this.Bodies.Count; i++)
{
// Point to body
RigidBody body = (RigidBody)this.Bodies[i];
// Process and drain applied impulse queue
body.FeelImpulses();
}
#endregion
/* // Dampening
* for (int i = 0; i < this.Bodies.Count; i++)
* {
* this.Bodies[i].Velocity *= 0.99f;
* this.Bodies[i].AngularVelocity *= 0.99f;
* } */
// Update elapsed scene time
this.elapsedSceneTime += minCollisionTime;
this.totalSceneTime += minCollisionTime;
// Check stopping criteria
if (this.totalSceneTime >= this.targetTotalTime)
{
stop = true;
}
// Substep override
if (this.Substep >= 10)
{
stop = true;
}
}
// Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0}", this.Name));
#endif
}
// =====
#region Update
// Update
public override void Update()
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0}", this.Name));
#endif
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(this.Name + " now starting update");
Log.Write(String.Format("Pre-update state: centroid = {0}, angle = {1:0.0000}",
this.Geometry.Centroid, this.Geometry.Angle));
Log.Write(String.Format("Pre-update state: bottom = {0:0.0000}, v = {1}, w = {2}",
this.Geometry.MinBoundingBox.Bottom, this.Velocity, this.AngularVelocity));
Log.Write("this.Contacts.Count = " + this.Contacts.Count);
}
#endif
// If at rest, go to exit
if (this.IsSleeping)
{
goto exit;
}
// If neither translatable nor rotatable, go to exit
if (!this.IsRotatable && !this.IsTranslatable)
{
goto exit;
}
// [Test] Dampen
this.angularVelocity *= 0.99f;
this.velocity *= 0.99f;
// ===
#region [1] Drain applied impulse queue
// Add force field impulses to queue
//this.GetFieldImpulses();
// Update velocities
//this.FeelImpulses();
// Add contact impulses to queue
//this.GetContactImpulses();
// Update velocities again
//this.FeelImpulses();
#endregion
// ===
#region [3] Check for collisions
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(this.Name + " now checking for collisions");
}
#endif
// Get elapsed scene time
float dt = (float)this.Scene.ElapsedSceneTime;
// Initialize overall minCollisionTime as elapsedSceneTime
float minCollisionTime = dt;
// Initialize list of earliest collisions
List <CollisionResult> collisions = new List <CollisionResult>();
// Loop through nearby objects
for (int i = 0; i < Globals.Scene.Bodies.Count; i++)
{
// If object is self, skip this iteration
if (Globals.Scene.Bodies[i].Equals(this))
{
continue;
}
// Perform a collision test
CollisionResult collisionResult = new CollisionResult(this, Globals.Scene.Bodies[i], this.Scene.TargetElapsedTime);
// Check if this collision occurs first
if (collisionResult.Time < minCollisionTime)
{
#region Case 1: Strictly first
// If so, update minCollisionTime and reset the list
minCollisionTime = collisionResult.Time;
collisions.Clear();
collisions.Add(collisionResult);
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(String.Format("This collision occurs first with t = {0:0.0000}, which is {1:00.00%} of dt", collisionResult.Time, collisionResult.Time / dt));
}
#endif
#endregion
}
// Otherwise, check if this collision is tied for first
else if (collisionResult.Time == minCollisionTime)
{
#region Case 2: Tied for first
// If so, add to list
collisions.Add(collisionResult);
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(String.Format("This collision is tied for first with t = {0:0.0000}", collisionResult.Time));
}
#endif
#endregion
}
// Otherwise, forget this collision
else
{
#region Case 3: Not first
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(String.Format("This collision does not occur first with t = {0:0.0000} > {1:0.0000}", collisionResult.Time, minCollisionTime));
}
#endif
#endregion
}
}
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(String.Format("{0} finished checking for collisions: count = {1}, minCollisionTime = {2:0.0000}, ", this.Name, collisions.Count, minCollisionTime));
}
#endif
#endregion
// ===
#region [4] Rotate and translate
// Translate and rotate shape
this.Geometry.RotateAndTranslateBy(
this.AngularVelocity * minCollisionTime,
this.RotationalAxis,
this.Velocity * minCollisionTime);
// Update rotational axis
this.RotationalAxis = this.Geometry.Centroid;
#endregion
// ===
#region [5] Get contact points
// Clear old contacts
this.Contacts.Clear();
// Loop through earliest collisions
for (int i = 0; i < collisions.Count; i++)
{
// Get contact points
Contact contact = new Contact(collisions[i]);
// Add to list
this.Contacts.Add(contact);
}
#endregion
// Add force field impulses to queue
this.GetFieldImpulses(this.Scene.ElapsedSceneTime);
// Update velocities
this.FeelImpulses();
// Add contact impulses to queue
// this.GetContactImpulses(this.Scene.ElapsedSceneTime);
// Update velocities again
this.FeelImpulses();
// ===
#region [*] Exit trap
exit:
// Null op
this.RotationalAxis = this.Geometry.Centroid;
// Physics logging
#if IS_LOGGING_PHYSICS
if (this.Name == Log.Subject1)
{
Log.Write(this.Name + " finished update");
Log.Write("==========");
}
#endif
// [*] Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0}", this.Name));
#endif
#endregion
}