// Apply force on a physical object
public override Impulse ApplyForceOn(RigidBody body)
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0} with {1}", this.Name, body.Name));
#endif
// Initialize result as zero
Impulse result = new Impulse(Vector2.Zero, Vector2.Zero);
// If the object is outside the field's area of effect, there is no force
if (!this.Geometry.IntersectsWith(body.Geometry.MinBoundingBox))
{
goto exit;
}
// For now, we will assume the object is wholly contained by the field,
// and hence there is only a translational force. In the future, we may
// consider torques exerted by 'partially contained' objects, e.g.
// sticking your arm out the window of a fast-moving car.
result.Momentum = this.CalculateForceOn(body.Mass, body.Geometry.Centroid);
result.Point = body.Geometry.Centroid;
// [*] Exit trap
exit:
// Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0} with {1}", this.Name, body.Name));
#endif
// Return result
return(result);
}
// Add field impulses to queue
public override void GetFieldImpulses(float dt)
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0}", this.Name));
#endif
// Loop through all force fields in scene
for (int i = 0; i < this.Scene.ForceFields.Count; i++)
{
// Get field impulse
Impulse impulse = this.Scene.ForceFields[i].GetImpulse(this, dt);
// If non-zero, apply it
if (impulse.Momentum != Vector2.Zero)
{
this.AppliedImpulses.Add(impulse);
}
}
// Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0}", this.Name));
#endif
}
// Decompose an impulse into a translational force and torque
public ForceAndTorque InterpretImpulse(Impulse appliedImpulse)
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0}", this.Name));
#endif
// Get force, the center-of-mass, translational force
Vector2 force = appliedImpulse.Momentum;
// Initialize torque as zero
float torque = 0;
// Point to impulse vector
Vector2 impulse = appliedImpulse.Momentum;
// Point to point of impact
Vector2 point = appliedImpulse.Point;
// If the point of impact is equal to centroid, e.g. an object wholly
// contained by a force field, then this is easy. There is no torque,
// only a translational force.
if (point == this.Geometry.Centroid)
{
goto exit;
}
// Get radialAxis, the pointOfImpact-to-rotationalAxis vector
Vector2 radialAxis = (this.RotationalAxis - point).Unit;
// Get leverArm, the length of this vector
float leverArm = (this.RotationalAxis - point).Length;
// Get the tangential force magnitude
float rotationalForce = Vector2.ComponentMagnitude(impulse, radialAxis.Normal);
// Compute the torque
torque = rotationalForce * leverArm;
// [*] Exit trap
exit:
// Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0}", this.Name));
#endif
// Return result
return(new ForceAndTorque(force, torque));
}
// =====
#region Methods
// If close enough, apply force on a physical object
public override Impulse ApplyForceOn(RigidBody body)
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0} with {1}", this.Name, body.Name));
#endif
// Initialize result as zero
Impulse result = new Impulse(Vector2.Zero, Vector2.Zero);
// Universal fields, by definition, wholly contain all objects
result.Momentum = this.CalculateForceOn(body.Mass, body.Geometry.Centroid);
result.Point = body.Geometry.Centroid;
// Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0} with {1}", this.Name, body.Name));
#endif
// Return result
return(result);
}
// (Old) Exert penalty forces to all bodies
public void Penalize()
{
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0}", this.Name);
#endif
if (this.Contacts.Count > 1)
{
#region Error: Multiple contacts not yet implemented
// Create error message
String s = "Contact error\n";
s += "So far, we can only handle singleton contact graphs!\n";
s += String.Format("Contacts.Count = ", this.Contacts.Count);
// Throw exception
throw new ArgumentException(s);
#endregion
}
else
{
// Not for collision contacts
if (!this.Contacts[0].IsResting)
{
return;
}
// Get bodyA
RigidBody bodyA = (RigidBody)this.Contacts[0].BodyA;
// Get bodyB
RigidBody bodyB = (RigidBody)this.Contacts[0].BodyB;
// Get edge-normal axis
Vector2 n = this.Contacts[0].Axis;
// Get contact point
Vector2 p = this.Contacts[0].Points[0];
#region Error: Edge-edge not yet implemented
#if IS_ERROR_CHECKING
// Check 'remembered' axes count
if (this.Contacts[0].Points.Count > 1)
{
if (false)
{
// Create error string
String s = "Contact error\n";
s += "Edge-edge contact resolution not yet implemented!\n";
s += String.Format("bodyA.Name = {0}, bodyB.Name = {1}\n", bodyA.Name, bodyB.Name);
// Throw exception
throw new SystemException(s);
}
}
#endif
#endregion
#region Get contact data
// Get elasticity
float e = 0.00f;
// Get mass
float mA = bodyA.Mass;
float mB = bodyB.Mass;
if (!bodyA.IsTranslatable)
{
mA = float.MaxValue;
}
if (!bodyB.IsTranslatable)
{
mB = float.MaxValue;
}
// Get moment of inertia
float IA = bodyA.MomentOfInertia;
float IB = bodyB.MomentOfInertia;
if (!bodyA.IsRotatable)
{
IA = float.MaxValue;
}
if (!bodyB.IsRotatable)
{
IB = float.MaxValue;
}
// Get centroid point
Vector2 cA = bodyA.Geometry.Centroid;
Vector2 cB = bodyB.Geometry.Centroid;
// Get centroid-to-point vector
Vector2 rA = p - cA;
Vector2 rB = p - cB;
// Get initial centroid velocity
Vector2 vA1 = bodyA.Velocity;
Vector2 vB1 = bodyB.Velocity;
// Get initial angular velocity
float wA1 = bodyA.AngularVelocity;
float wB1 = bodyB.AngularVelocity;
// Get initial point velocity
Vector2 vA1p = vA1 - wA1 * rA.Normal;
Vector2 vB1p = vB1 - wB1 * rB.Normal;
// Get initial point velocity projected
float vA1pn = Vector2.Project(vA1p, n);
float vB1pn = Vector2.Project(vB1p, n);
// Get initial point velocity projected relative
float vR1pn = vA1pn - vB1pn;
#endregion
#region Calculate impulses
// Calculate the impulse parameter j
// float j = (-1 * (1 + e) * vR1pn) / ((1 / mA) + (1 / mB) + ((float)Math.Pow((rA.X * n.Y - rA.Y * n.X), 2) / IA) + ((float)Math.Pow((rB.X * n.Y - rB.Y * n.X), 2) / IB));
float kp = 50 * 6000.0f;
float kv = 50 * 3000.0f;
float d = this.Contacts[0].Distance;
float j = kp * (-d) + kv * (-vR1pn);
// Make sure the bodies aren't actually moving away from each other
if (vR1pn > 0)
{
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (bodyA.Name == Log.Subject1 && (Log.Subject2 == null || bodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("j = {0}, but we set it to zero since vRpn = {1}", j, vA1pn, vR1pn));
}
#endif
j = 0;
}
// Get final centroid velocity
Vector2 vA2 = vA1 + (j / mA) * n;
Vector2 vB2 = vB1 - (j / mB) * n;
// Get impulse on object A
Vector2 F = mA * (vA2 - vA1);
// Get final angular velocity
float wA2 = wA1 + (rA.X * j * n.Y - rA.Y * j * n.X) / IA;
float wB2 = wB1 - (rB.X * j * n.Y - rB.Y * j * n.X) / IB;
// Get final point velocity
Vector2 vA2p = vA2 - wA2 * rA.Normal;
Vector2 vB2p = vB2 - wB2 * rB.Normal;
// Get final point velocity projected
float vA2pn = Vector2.Project(vA2p, n);
float vB2pn = Vector2.Project(vB2p, n);
// Get final point velocity projected relative
float vR2pn = vA2pn - vB2pn;
// Get initial point acceleration (VERIFY)
Vector2 aA1p = -wA1 * rA.Normal - wA1 * rA;
Vector2 aB1p = -wB1 * rB.Normal - wB1 * rB;
// Get initial point acceleration projected
float aA1pn = Vector2.Project(aA1p, n);
float aB1pn = Vector2.Project(aB1p, n);
// Get initial point acceleration projected relative
float aR1pn = aA1pn - aB1pn;
// Get initial contact edge
LineSegment edge = this.Contacts[0].Edge;
// Get edge point 1 derivative
Vector2 e1dot = -wB1 * (edge.Point1 - cB).Normal + vB1;
// Get edge point 2 derivative
Vector2 e2dot = -wB1 * (edge.Point2 - cB).Normal + vB1;
// Get edge normal derivative
Vector2 ndot = (1 / edge.Length) * new Vector2(e2dot.Y - e1dot.Y, e1dot.X - e2dot.X);
// Get initial point 1st derivative
Vector2 pA1pdot = -wA1 * rA.Normal + vA1;
Vector2 pB1pdot = -wB1 * rB.Normal + vB1;
// Get initial point 2nd derivative
Vector2 pA1pdot2 = -wA1 * rA.Normal + wA1 * rA + Vector2.Zero;
Vector2 pB1pdot2 = -wB1 * rB.Normal + wB1 * rB + Vector2.Zero;
// Get initial contact distance
float d1 = this.Contacts[0].Distance;
// Get initial contact distance 1st derivative
float d1dot = Vector2.Project(Vector2.Zero, ndot) + Vector2.Project(pA1pdot - pB1pdot, n);
// Get initial contact distance 2nd derivative
float d1dot2 = Vector2.Dot(pA1pdot2 - pB1pdot2, n) + 2 * Vector2.Dot(pA1pdot - pB1pdot, ndot);
// Equation: d_i = a_ij * f + b_i
// d_i = contact distance acceleration
// a_ij = force-dependent terms
// f = scalar force
// b_i = force-independent terms
// Get 'a_ij'
// Get 'b_i'
#endregion
#region Log summary
// Test logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (bodyA.Name == Log.Subject1 || bodyB.Name == Log.Subject1) && (Log.Subject2 == null || (bodyA.Name == Log.Subject2 || bodyB.Name == Log.Subject2)))
{
Log.Write("Now beginning collision response summary...");
Log.Write(String.Format("bodyA = {0}, bodyB = {1}, isResting = {2}", bodyA.Name, bodyB.Name, this.Contacts[0].IsResting));
Log.Write(String.Format("n = {0}, p = {1}, e = {2}", n, p, e));
Log.Write(String.Format("vA1 = {0}, vB1 = {1}", vA1, vB1));
Log.Write(String.Format("vA2 = {0}, vB2 = {1}", vA2, vB2));
Log.Write(String.Format("wA1 = {0:+0000.0000;-0000.0000; 0000.0000}, wB1 = {1:+0000.0000;-0000.0000; 0000.0000}", wA1, wB1));
Log.Write(String.Format("wA2 = {0:+0000.0000;-0000.0000; 0000.0000}, wB2 = {1:+0000.0000;-0000.0000; 0000.0000}", wA2, wB2));
Log.Write(String.Format("vA1p = {0}, vB1p = {1}", vA1p, vB1p));
Log.Write(String.Format("vA2p = {0}, vB2p = {1}", vA2p, vB2p));
Log.Write(String.Format("vA1pn = {0:+0000.0000;-0000.0000; 0000.0000}, vB1pn = {1:+0000.0000;-0000.0000; 0000.0000}", vA1pn, vB1pn));
Log.Write(String.Format("vA2pn = {0:+0000.0000;-0000.0000; 0000.0000}, vB2pn = {1:+0000.0000;-0000.0000; 0000.0000}", vA2pn, vB2pn));
Log.Write(String.Format("vR1pn = {0:+0000.0000;-0000.0000; 0000.0000}", vR1pn));
Log.Write(String.Format("vR2pn = {0:+0000.0000;-0000.0000; 0000.0000}", vR2pn));
/* Log.Write(String.Format("j = {0:+0.0000E+0;-0.0000E+0;+0.0000E+0}, F = {1:+0.0000E+0;-0.0000E+0;+0.0000E+0}", j, F));
*
* Log.Write(String.Format("aA1p = {0}", aA1p));
* Log.Write(String.Format("aB1p = {0}", aB1p));
* Log.Write(String.Format("aA1pn = {0:+0000.0000;-0000.0000; 0000.0000}", aA1pn));
* Log.Write(String.Format("aB1pn = {0:+0000.0000;-0000.0000; 0000.0000}", aB1pn));
* Log.Write(String.Format("aR1pn = {0:+0000.0000;-0000.0000; 0000.0000}", aR1pn));
*
* Log.Write(String.Format("edge = {0}", edge));
* Log.Write(String.Format("e1dot = {0}", e1dot));
* Log.Write(String.Format("e2dot = {0}", e2dot));
* Log.Write(String.Format("ndot = {0}", ndot));
*
* Log.Write(String.Format("pA1pdot = {0}, pB1pdot = {1}", pA1pdot, pB1pdot));
* Log.Write(String.Format("pA1pdot2 = {0}, pB1pdot2 = {1}", pA1pdot2, pB1pdot2));
* Log.Write(String.Format("d1 = {0:+0000.0000;-0000.0000; 0000.0000}", d1));
* Log.Write(String.Format("d1dot = {0:+0000.0000;-0000.0000; 0000.0000}", d1dot));
* Log.Write(String.Format("d1dot2 = {0:+0000.0000;-0000.0000; 0000.0000}", d1dot2)); */
}
#endif
#endregion
// Create contact impulse
Impulse contactImpulse = new Impulse(F, p);
// Feel neighbor's normal force
bodyA.AppliedImpulses.Add(contactImpulse);
// Negate contact impulse
contactImpulse = new Impulse(-F, p);
// Exert normal force on neighbor
bodyB.AppliedImpulses.Add(contactImpulse);
}
// Exit logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Exiting method for {0}", this.Name);
#endif
}
