/*
* Position Correction Notes
* =========================
* I tried the several algorithms for position correction of the 2D revolute joint.
* I looked at these systems:
* - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
* - suspension bridge with 30 1m long planks of length 1m.
* - multi-link chain with 30 1m long links.
*
* Here are the algorithms:
*
* Baumgarte - A fraction of the position error is added to the velocity error. There is no
* separate position solver.
*
* Pseudo Velocities - After the velocity solver and position integration,
* the position error, Jacobian, and effective mass are recomputed. Then
* the velocity constraints are solved with pseudo velocities and a fraction
* of the position error is added to the pseudo velocity error. The pseudo
* velocities are initialized to zero and there is no warm-starting. After
* the position solver, the pseudo velocities are added to the positions.
* This is also called the First Order World method or the Position LCP method.
*
* Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
* position error is re-computed for each constraint and the positions are updated
* after the constraint is solved. The radius vectors (aka Jacobians) are
* re-computed too (otherwise the algorithm has horrible instability). The pseudo
* velocity states are not needed because they are effectively zero at the beginning
* of each iteration. Since we have the current position error, we allow the
* iterations to terminate early if the error becomes smaller than b2_linearSlop.
*
* Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
* each time a constraint is solved.
*
* Here are the results:
* Baumgarte - this is the cheapest algorithm but it has some stability problems,
* especially with the bridge. The chain links separate easily close to the root
* and they jitter as they struggle to pull together. This is one of the most common
* methods in the field. The big drawback is that the position correction artificially
* affects the momentum, thus leading to instabilities and false bounce. I used a
* bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
* factor makes joints and contacts more spongy.
*
* Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
* stable. However, joints still separate with large angular velocities. Drag the
* simple pendulum in a circle quickly and the joint will separate. The chain separates
* easily and does not recover. I used a bias factor of 0.2. A larger value lead to
* the bridge collapsing when a heavy cube drops on it.
*
* Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
* Velocities, but in other ways it is worse. The bridge and chain are much more
* stable, but the simple pendulum goes unstable at high angular velocities.
*
* Full NGS - stable in all tests. The joints display good stiffness. The bridge
* still sags, but this is better than infinite forces.
*
* Recommendations
* Pseudo Velocities are not really worthwhile because the bridge and chain cannot
* recover from joint separation. In other cases the benefit over Baumgarte is small.
*
* Modified NGS is not a robust method for the revolute joint due to the violent
* instability seen in the simple pendulum. Perhaps it is viable with other constraint
* types, especially scalar constraints where the effective mass is a scalar.
*
* This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
* and is very fast. I don't think we can escape Baumgarte, especially in highly
* demanding cases where high constraint fidelity is not needed.
*
* Full NGS is robust and easy on the eyes. I recommend this as an option for
* higher fidelity simulation and certainly for suspension bridges and long chains.
* Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
* joint separation can be problematic. The number of NGS iterations can be reduced
* for better performance without harming robustness much.
*
* Each joint in a can be handled differently in the position solver. So I recommend
* a system where the user can select the algorithm on a per joint basis. I would
* probably default to the slower Full NGS and let the user select the faster
* Baumgarte method in performance critical scenarios.
*/
/*
* Cache Performance
*
* The Box2D solvers are dominated by cache misses. Data structures are designed
* to increase the number of cache hits. Much of misses are due to random access
* to body data. The constraint structures are iterated over linearly, which leads
* to few cache misses.
*
* The bodies are not accessed during iteration. Instead read only data, such as
* the mass values are stored with the constraints. The mutable data are the constraint
* impulses and the bodies velocities/positions. The impulses are held inside the
* constraint structures. The body velocities/positions are held in compact, temporary
* arrays to increase the number of cache hits. Linear and angular velocity are
* stored in a single array since multiple arrays lead to multiple misses.
*/
/*
* 2D Rotation
*
* R = [cos(theta) -sin(theta)]
* [sin(theta) cos(theta) ]
*
* thetaDot = omega
*
* Let q1 = cos(theta), q2 = sin(theta).
* R = [q1 -q2]
* [q2 q1]
*
* q1Dot = -thetaDot * q2
* q2Dot = thetaDot * q1
*
* q1_new = q1_old - dt * w * q2
* q2_new = q2_old + dt * w * q1
* then normalize.
*
* This might be faster than computing sin+cos.
* However, we can compute sin+cos of the same angle fast.
*/
public b2Island(int bodyCapacity, int contactCapacity, int jointCapacity, b2ContactListener listener)
{
m_bodyCapacity = bodyCapacity;
m_contactCapacity = contactCapacity;
m_jointCapacity = jointCapacity;
m_bodyCount = 0;
m_contactCount = 0;
m_jointCount = 0;
m_listener = listener;
m_bodies = new b2Body[bodyCapacity];
m_contacts = new b2Contact[contactCapacity];
m_joints = new b2Joint[jointCapacity];
m_velocities = Arrays.InitializeWithDefaultInstances <b2Velocity>(m_bodyCapacity);
m_positions = Arrays.InitializeWithDefaultInstances <b2Position>(m_bodyCapacity);
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
public virtual void Update(b2ContactListener listener)
{
oldManifold.CopyFrom(m_manifold);
// Re-enable this contact.
Flags |= b2ContactFlags.e_enabledFlag;
bool touching = false;
bool wasTouching = (Flags & b2ContactFlags.e_touchingFlag) == b2ContactFlags.e_touchingFlag;
bool sensor = FixtureA.m_isSensor || FixtureB.m_isSensor;
b2Body bodyA = FixtureA.Body;
b2Body bodyB = FixtureB.Body;
b2Transform xfA = bodyA.Transform;
b2Transform xfB = bodyB.Transform;
// Is this contact a sensor?
if (sensor)
{
b2Shape shapeA = FixtureA.Shape;
b2Shape shapeB = FixtureB.Shape;
touching = b2Collision.b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
}
else
{
Evaluate(m_manifold, ref xfA, ref xfB);
touching = m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.pointCount; ++i)
{
b2ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
b2ContactFeature id2 = mp2.id;
for (int j = 0; j < oldManifold.pointCount; ++j)
{
b2ManifoldPoint mp1 = oldManifold.points[j];
if (mp1.id.key == id2.key)
{
mp2.normalImpulse = mp1.normalImpulse;
mp2.tangentImpulse = mp1.tangentImpulse;
break;
}
}
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
Flags |= b2ContactFlags.e_touchingFlag;
}
else
{
Flags &= ~b2ContactFlags.e_touchingFlag;
}
if (wasTouching == false && touching == true && listener != null)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && listener != null)
{
listener.EndContact(this);
}
if (sensor == false && touching && listener != null)
{
listener.PreSolve(this, oldManifold);
}
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
public virtual void Update(b2ContactListener listener)
{
b2Manifold oldManifold = m_manifold;
oldManifold.points = m_manifold.CopyPoints();
// Re-enable this contact.
m_flags |= b2ContactFlags.e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & b2ContactFlags.e_touchingFlag) == b2ContactFlags.e_touchingFlag;
bool sensorA = m_fixtureA.IsSensor;
bool sensorB = m_fixtureB.IsSensor;
bool sensor = sensorA || sensorB;
b2Body bodyA = m_fixtureA.Body;
b2Body bodyB = m_fixtureB.Body;
b2Transform xfA = bodyA.Transform;
b2Transform xfB = bodyB.Transform;
// Is this contact a sensor?
if (sensor)
{
b2Shape shapeA = m_fixtureA.Shape;
b2Shape shapeB = m_fixtureB.Shape;
touching = b2Collision.b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
}
else
{
Evaluate(ref m_manifold, ref xfA, ref xfB);
touching = m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.pointCount; ++i)
{
b2ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
b2ContactFeature id2 = mp2.id;
for (int j = 0; j < oldManifold.pointCount; ++j)
{
b2ManifoldPoint mp1 = oldManifold.points[j];
if (mp1.id.key == id2.key)
{
mp2.normalImpulse = mp1.normalImpulse;
mp2.tangentImpulse = mp1.tangentImpulse;
break;
}
}
m_manifold.points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
m_flags |= b2ContactFlags.e_touchingFlag;
}
else
{
m_flags &= ~b2ContactFlags.e_touchingFlag;
}
if (wasTouching == false && touching == true && listener != null)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && listener != null)
{
listener.EndContact(this);
}
if (sensor == false && touching && listener != null)
{
listener.PreSolve(this, ref oldManifold);
}
}
public void SetContactListener(b2ContactListener listener)
{
Box2dPINVOKE.b2World_SetContactListener(swigCPtr, b2ContactListener.getCPtr(listener));
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(b2ContactListener listener)
{
b2Manifold oldManifold = m_manifold;
// Re-enable this contact.
m_flags |= ContactFlags.e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & ContactFlags.e_touchingFlag) == ContactFlags.e_touchingFlag;
bool sensorA = m_fixtureA.IsSensor();
bool sensorB = m_fixtureB.IsSensor();
bool sensor = sensorA || sensorB;
b2Body bodyA = m_fixtureA.GetBody();
b2Body bodyB = m_fixtureB.GetBody();
b2Transform xfA = bodyA.GetTransform();
b2Transform xfB = bodyB.GetTransform();
// Is this contact a sensor?
if (sensor)
{
b2Shape shapeA = m_fixtureA.GetShape();
b2Shape shapeB = m_fixtureB.GetShape();
touching = Utils.b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
}
else
{
Evaluate(m_manifold, xfA, xfB);
touching = m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.pointCount; ++i)
{
b2ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
b2ContactID id2 = mp2.id;
for (int j = 0; j < oldManifold.pointCount; ++j)
{
b2ManifoldPoint mp1 = oldManifold.points[j];
if (mp1.id.key == id2.key)
{
mp2.normalImpulse = mp1.normalImpulse;
mp2.tangentImpulse = mp1.tangentImpulse;
break;
}
}
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
m_flags |= ContactFlags.e_touchingFlag;
}
else
{
m_flags &= ~ContactFlags.e_touchingFlag;
}
if (wasTouching == false && touching == true && listener != null)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && listener != null)
{
listener.EndContact(this);
}
if (sensor == false && touching && listener != null)
{
listener.PreSolve(this, oldManifold);
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2ContactListener obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public void Update(b2ContactListener listener)
{
// Swap old & new manifold
b2Manifold tManifold = m_oldManifold;
m_oldManifold = m_manifold;
m_manifold = tManifold;
// Re-enable this contact
m_flags |= e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
b2Body bodyA = m_fixtureA.m_body;
b2Body bodyB = m_fixtureB.m_body;
bool aabbOverlap = m_fixtureA.m_aabb.TestOverlap(m_fixtureB.m_aabb);
// Is this contat a sensor?
if ((m_flags & e_sensorFlag) > 0)
{
if (aabbOverlap)
{
b2Shape shapeA = m_fixtureA.GetShape();
b2Shape shapeB = m_fixtureB.GetShape();
b2Transform xfA = bodyA.GetTransform();
b2Transform xfB = bodyB.GetTransform();
touching = b2Shape.TestOverlap(shapeA, xfA, shapeB, xfB);
}
// Sensors don't generate manifolds
m_manifold.m_pointCount = 0;
}
else
{
// Slow contacts don't generate TOI events.
if (bodyA.GetType() != b2Body.b2_dynamicBody || bodyA.IsBullet() || bodyB.GetType() != b2Body.b2_dynamicBody || bodyB.IsBullet())
{
m_flags |= e_continuousFlag;
}
else
{
m_flags &= ~e_continuousFlag;
}
if (aabbOverlap)
{
Evaluate();
touching = m_manifold.m_pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.m_pointCount; ++i)
{
b2ManifoldPoint mp2 = m_manifold.m_points[i];
mp2.m_normalImpulse = 0.0f;
mp2.m_tangentImpulse = 0.0f;
b2ContactID id2 = mp2.m_id;
for (int j = 0; j < m_oldManifold.m_pointCount; ++j)
{
b2ManifoldPoint mp1 = m_oldManifold.m_points[j];
if (mp1.m_id.key == id2.key)
{
mp2.m_normalImpulse = mp1.m_normalImpulse;
mp2.m_tangentImpulse = mp1.m_tangentImpulse;
break;
}
}
}
}
else
{
m_manifold.m_pointCount = 0;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
m_flags |= e_touchingFlag;
}
else
{
m_flags &= ~e_touchingFlag;
}
if (wasTouching == false && touching == true)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false)
{
listener.EndContact(this);
}
if ((m_flags & e_sensorFlag) == 0)
{
listener.PreSolve(this, m_oldManifold);
}
}