public void Initialize(Contact[] contacts, int count, Body toiBody)
{
_count = count;
_toiBody = toiBody;
if (_constraints.Length < _count)
{
_constraints = new TOIConstraint[Math.Max(_constraints.Length * 2, _count)];
}
for (int i = 0; i < _count; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.GetFixtureA();
Fixture fixtureB = contact.GetFixtureB();
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold;
contact.GetManifold(out manifold);
//Debug.Assert(manifold._pointCount > 0);
TOIConstraint constraint = _constraints[i];
constraint.bodyA = bodyA;
constraint.bodyB = bodyB;
constraint.localNormal = manifold._localNormal;
constraint.localPoint = manifold._localPoint;
constraint.type = manifold._type;
constraint.pointCount = manifold._pointCount;
constraint.radius = radiusA + radiusB;
for (int j = 0; j < constraint.pointCount; ++j)
{
constraint.localPoints[j] = manifold._points[j].LocalPoint;
}
_constraints[i] = constraint;
}
}
/// Called for each fixture found in the query AABB.
/// @return false to terminate the query.
public override bool ReportFixture(Fixture fixture)
{
if (m_count == e_maxCount)
{
return false;
}
Body body = fixture.GetBody();
Shape shape = fixture.GetShape();
bool overlap = Collision.TestOverlap(shape, 0, m_circle, 0, body.GetTransform(), m_transform);
if (overlap)
{
DrawFixture(fixture);
++m_count;
}
return true;
}
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction)
{
Body body = fixture.GetBody();
if (body.UserData != null)
{
int index = (int)body.UserData;
if (index == 0)
{
// By returning -1, we instruct the calling code to ignore this fixture
// and continue the ray-cast to the next fixture.
return -1.0f;
}
}
m_hit = true;
m_point = point;
m_normal = normal;
// At this point we have a hit, so we know the ray is obstructed.
// By returning 0, we instruct the calling code to terminate the ray-cast.
return 0.0f;
}
public void DrawFixture(Fixture fixture)
{
Color color = Color.FromArgb(245, 245, 150);
Transform xf = fixture.GetBody().GetTransform();
switch (fixture.GetShapeType())
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.GetShape();
Vec2 center = Utilities.Mul(xf, circle.m_p);
float radius = circle.m_radius;
m_debugDraw.DrawCircle(center, radius, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.GetShape();
int vertexCount = poly.m_count;
Utilities.Assert(vertexCount <= Settings._maxPolygonVertices);
Vec2[] vertices = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Utilities.Mul(xf, poly.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, vertexCount, color);
}
break;
default:
break;
}
}
public void Destroy(Contact c)
{
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (m_contactListener != null && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
m_contactList.Remove(c);
// Remove from body 1
c.m_nodeA.other.m_contactList.Remove(c.m_nodeA);
// Remove from body 2
c.m_nodeB.other.m_contactList.Remove(c.m_nodeA);
// Call the factory.
}
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction)
{
Body body = fixture.GetBody();
if (body.UserData != null)
{
int index = (int)body.UserData;
if (index == 0)
{
// By returning -1, we instruct the calling code to ignore this fixture and
// continue the ray-cast to the next fixture.
return -1.0f;
}
}
m_hit = true;
m_point = point;
m_normal = normal;
// By returning the current fraction, we instruct the calling code to clip the ray and
// continue the ray-cast to the next fixture. WARNING: do not assume that fixtures
// are reported in order. However, by clipping, we can always get the closest fixture.
return fraction;
}
internal void Collide()
{
// Update awake contacts.
Contact c = _contactList;
while (c != null)
{
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (bodyA.IsAwake() == false && bodyB.IsAwake() == false)
{
c = c.GetNext();
continue;
}
// Is this contact flagged for filtering?
if ((c._flags & ContactFlags.Filter) == ContactFlags.Filter)
{
// Should these bodies collide?
if (bodyB.ShouldCollide(bodyA) == false)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c._flags &= ~ContactFlags.Filter;
}
int proxyIdA = fixtureA._proxies[indexA].proxyId;
int proxyIdB = fixtureB._proxies[indexB].proxyId;
bool overlap = _broadPhase.TestOverlap(proxyIdA, proxyIdB);
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// The contact persists.
c.Update(ContactListener);
c = c.GetNext();
}
}
internal void Destroy(Contact c)
{
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (ContactListener != null && c.IsTouching())
{
ContactListener.EndContact(c);
}
// Remove from the world.
if (c._prev != null)
{
c._prev._next = c._next;
}
if (c._next != null)
{
c._next._prev = c._prev;
}
if (c == _contactList)
{
_contactList = c._next;
}
// Remove from body 1
if (c._nodeA.Prev != null)
{
c._nodeA.Prev.Next = c._nodeA.Next;
}
if (c._nodeA.Next != null)
{
c._nodeA.Next.Prev = c._nodeA.Prev;
}
if (c._nodeA == bodyA._contactList)
{
bodyA._contactList = c._nodeA.Next;
}
// Remove from body 2
if (c._nodeB.Prev != null)
{
c._nodeB.Prev.Next = c._nodeB.Next;
}
if (c._nodeB.Next != null)
{
c._nodeB.Next.Prev = c._nodeB.Prev;
}
if (c._nodeB == bodyB._contactList)
{
bodyB._contactList = c._nodeB.Next;
}
c.Destroy();
--_contactCount;
}
// Broad-phase callback.
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
FixtureProxy proxyA = (FixtureProxy)proxyUserDataA;
FixtureProxy proxyB = (FixtureProxy)proxyUserDataB;
Fixture fixtureA = proxyA.fixture;
Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
List <ContactEdge> edges = bodyB.GetContactList();
foreach (ContactEdge edge in edges)
{
if (edge.other == bodyA)
{
Fixture fA = edge.contact.FixtureA;
Fixture fB = edge.contact.FixtureB;
int iA = edge.contact.GetChildIndexA();
int iB = edge.contact.GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if ((m_contactFilter != null) && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
indexA = c.GetChildIndexA();
indexB = c.GetChildIndexB();
bodyA = fixtureA.GetBody();
bodyB = fixtureB.GetBody();
// Insert into the world.
m_contactList.Add(c);
// Connect to island graph.
// Connect to body A
c.m_nodeA.contact = c;
c.m_nodeA.other = bodyB;
bodyA.m_contactList.Add(c.m_nodeA);
// Connect to body B
c.m_nodeB.contact = c;
c.m_nodeB.other = bodyA;
bodyB.m_contactList.Add(c.m_nodeB);
// Wake up the bodies
if (fixtureA.IsSensor == false && fixtureB.IsSensor == false)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
public void Collide()
{
// Update awake contacts.
for (int i = 0; i < m_contactList.Count(); i++)
{
Contact c = m_contactList[i];
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
// Is this contact flagged for filtering?
if (c.m_flags.HasFlag(ContactFlags.e_filterFlag))
{
throw new NotImplementedException();
//// Should these bodies collide?
//if (bodyB.ShouldCollide(bodyA) == false)
//{
// Contact cNuke = c;
// c = cNuke.GetNext();
// Destroy(cNuke);
// continue;
//}
//// Check user filtering.
//if (m_contactFilter && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
//{
// Contact* cNuke = c;
// c = cNuke.GetNext();
// Destroy(cNuke);
// continue;
//}
//// Clear the filtering flag.
//c.m_flags &= ~ContactFlags.e_filterFlag;
}
bool activeA = bodyA.IsAwake() && bodyA.m_type != BodyType._staticBody;
bool activeB = bodyB.IsAwake() && bodyB.m_type != BodyType._staticBody;
// At least one body must be awake and it must be dynamic or kinematic.
if (activeA == false && activeB == false)
{
continue;
}
int proxyIdA = fixtureA.m_proxies[indexA].proxyId;
int proxyIdB = fixtureB.m_proxies[indexB].proxyId;
bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
Destroy(c);
continue;
}
// The contact persists.
c.Update(m_contactListener);
}
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(ContactListener listener)
{
Manifold 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;
Body bodyA = m_fixtureA.GetBody();
Body bodyB = m_fixtureB.GetBody();
Transform xfA = bodyA.GetTransform();
Transform xfB = bodyB.GetTransform();
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = m_fixtureA.GetShape();
Shape shapeB = m_fixtureB.GetShape();
touching = Collision.TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
// Sensors don't generate manifolds.
m_manifold.points.Clear();
}
else
{
Evaluate(out m_manifold, xfA, xfB);
touching = m_manifold.points.Count() > 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.points.Count(); ++i)
{
ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
ContactID id2 = mp2.id;
for (int j = 0; j < oldManifold.points.Count(); ++j)
{
ManifoldPoint 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);
}
}
public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.GetType() != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.dt * b._invI * b._torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < _contactCount; ++i2)
{
Fixture fixtureA = _contacts[i2].GetFixtureA();
Fixture fixtureB = _contacts[i2].GetFixtureB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static;
if (nonStatic)
{
++i1;
//b2Swap(_contacts[i1], _contacts[i2]);
Contact temp = _contacts[i1];
_contacts[i1] = _contacts[i2];
_contacts[i2] = temp;
}
}
// Initialize velocity constraints.
_contactSolver.Reset(_contacts, _contactCount, step.dtRatio);
_contactSolver.WarmStart();
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(ref step);
}
// Solve velocity constraints.
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(ref step);
}
_contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
_contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.GetType() == BodyType.Static)
{
continue;
}
// Check for large velocities.
Vector2 translation = step.dt * b._linearVelocity;
if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared)
{
float ratio = Settings.b2_maxTranslation / translation.magnitude;
b._linearVelocity *= ratio;
}
float rotation = step.dt * b._angularVelocity;
if (rotation * rotation > Settings.b2_maxRotationSquared)
{
float ratio = Settings.b2_maxRotation / Math.Abs(rotation);
b._angularVelocity *= ratio;
}
// Store positions for continuous collision.
b._sweep.c0 = b._sweep.c;
b._sweep.a0 = b._sweep.a;
// Integrate
b._sweep.c += step.dt * b._linearVelocity;
b._sweep.a += step.dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(_contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.b2_maxFloat;
const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.GetType() == BodyType.Static)
{
continue;
}
if ((b._flags & BodyFlags.AutoSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & BodyFlags.AutoSleep) == 0 ||
b._angularVelocity * b._angularVelocity > angTolSqr ||
Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.b2_timeToSleep)
{
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
b.SetAwake(false);
}
}
}
}
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction)
{
Body body = fixture.GetBody();
if (body.UserData != null)
{
int index = (int)body.UserData;
if (index == 0)
{
// By returning -1, we instruct the calling code to ignore this fixture
// and continue the ray-cast to the next fixture.
return -1.0f;
}
}
Utilities.Assert(m_count < e_maxCount);
m_points[m_count] = point;
m_normals[m_count] = normal;
++m_count;
if (m_count == e_maxCount)
{
// At this point the buffer is full.
// By returning 0, we instruct the calling code to terminate the ray-cast.
return 0.0f;
}
// By returning 1, we instruct the caller to continue without clipping the ray.
return 1.0f;
}
private void SolveTOI(TimeStep step)
{
Island island = new Island(m_contactManager.m_contactListener);
if (m_stepComplete)
{
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
foreach (Contact c in m_contactManager.m_contactList)
{
// Invalidate TOI
c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
Fixture fA = null;
Fixture fB = null;
Body bA = null;
Body bB = null;
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
foreach (Contact c in m_contactManager.m_contactList)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > Settings._maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.m_flags.HasFlag(ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
fA = c.FixtureA;
fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
bA = fA.GetBody();
bB = fB.GetBody();
BodyType typeA = bA.m_type;
BodyType typeB = bB.m_type;
Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody);
bool activeA = bA.IsAwake() && typeA != BodyType._staticBody;
bool activeB = bB.IsAwake() && typeB != BodyType._staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody;
bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.m_sweep.alpha0;
if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
{
alpha0 = bB.m_sweep.alpha0;
bA.m_sweep.Advance(alpha0);
}
else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
{
alpha0 = bA.m_sweep.alpha0;
bB.m_sweep.Advance(alpha0);
}
Utilities.Assert(alpha0 < 1.0f);
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fA.GetShape(), indexA);
input.proxyB.Set(fB.GetShape(), indexB);
input.sweepA = bA.m_sweep;
input.sweepB = bB.m_sweep;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == TOIOutput.State.e_touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.m_flags |= ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
fA = minContact.FixtureA;
fB = minContact.FixtureB;
bA = fA.GetBody();
bB = fB.GetBody();
Sweep backup1 = bA.m_sweep;
Sweep backup2 = bB.m_sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.m_contactListener);
minContact.m_flags &= ~ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.m_sweep = backup1;
bB.m_sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.m_flags |= Body.BodyFlags.e_islandFlag;
bB.m_flags |= Body.BodyFlags.e_islandFlag;
minContact.m_flags |= ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA, bB };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.m_type == BodyType._dynamicBody)
{
foreach (ContactEdge ce in body.m_contactList)
{
throw new NotImplementedException();
//if (island.m_bodies.Count() == island.m_bodyCapacity)
//{
// break;
//}
//if (island.m_bodies.Count() == island.m_contactCapacity)
//{
// break;
//}
//Contact* contact = ce.contact;
//// Has this contact already been added to the island?
//if (contact.m_flags & ContactFlags.e_islandFlag)
//{
// continue;
//}
//// Only add static, kinematic, or bullet bodies.
//Body* other = ce.other;
//if (other.m_type == _dynamicBody &&
// body.IsBullet() == false && other.IsBullet() == false)
//{
// continue;
//}
//// Skip sensors.
//bool sensorA = contact.m_fixtureA.m_isSensor;
//bool sensorB = contact.m_fixtureB.m_isSensor;
//if (sensorA || sensorB)
//{
// continue;
//}
//// Tentatively advance the body to the TOI.
//Sweep backup = other.m_sweep;
//if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0)
//{
// other.Advance(minAlpha);
//}
//// Update the contact points
//contact.Update(m_contactManager.m_contactListener);
//// Was the contact disabled by the user?
//if (contact.IsEnabled() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Are there contact points?
//if (contact.IsTouching() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Add the contact to the island
//contact.m_flags |= ContactFlags.e_islandFlag;
//island.Add(contact);
//// Has the other body already been added to the island?
//if (other.m_flags & Body.BodyFlags.e_islandFlag)
//{
// continue;
//}
//// Add the other body to the island.
//other.m_flags |= Body.BodyFlags.e_islandFlag;
//if (other.m_type != _staticBody)
//{
// other.SetAwake(true);
//}
//island.Add(other);
}
}
}
TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodies.Count(); ++i)
{
throw new NotImplementedException();
//Body* body = island.m_bodies[i];
//body.m_flags &= ~Body.BodyFlags.e_islandFlag;
//if (body.m_type != _dynamicBody)
//{
// continue;
//}
//body.SynchronizeFixtures();
//// Invalidate all contact TOIs on this displaced body.
//for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next)
//{
// ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
//}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
// Broad-phase callback.
internal void AddPair(FixtureProxy proxyA, FixtureProxy proxyB)
{
Fixture fixtureA = proxyA.fixture;
Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
ContactEdge edge = bodyB.GetContactList();
while (edge != null)
{
if (edge.Other == bodyA)
{
Fixture fA = edge.Contact.GetFixtureA();
Fixture fB = edge.Contact.GetFixtureB();
int iA = edge.Contact.GetChildIndexA();
int iB = edge.Contact.GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
// Contact creation may swap fixtures.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
indexA = c.GetChildIndexA();
indexB = c.GetChildIndexB();
bodyA = fixtureA.GetBody();
bodyB = fixtureB.GetBody();
// Insert into the world.
c._prev = null;
c._next = _contactList;
if (_contactList != null)
{
_contactList._prev = c;
}
_contactList = c;
// Connect to island graph.
// Connect to body A
c._nodeA.Contact = c;
c._nodeA.Other = bodyB;
c._nodeA.Prev = null;
c._nodeA.Next = bodyA._contactList;
if (bodyA._contactList != null)
{
bodyA._contactList.Prev = c._nodeA;
}
bodyA._contactList = c._nodeA;
// Connect to body B
c._nodeB.Contact = c;
c._nodeB.Other = bodyA;
c._nodeB.Prev = null;
c._nodeB.Next = bodyB._contactList;
if (bodyB._contactList != null)
{
bodyB._contactList.Prev = c._nodeB;
}
bodyB._contactList = c._nodeB;
++_contactCount;
}
public void Reset(Contact[] contacts, int contactCount, float impulseRatio)
{
_contacts = contacts;
_constraintCount = contactCount;
// grow the array
if (_constraints == null || _constraints.Length < _constraintCount)
{
_constraints = new ContactConstraint[_constraintCount * 2];
}
for (int i = 0; i < _constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold;
contact.GetManifold(out manifold);
float friction = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
//Debug.Assert(manifold._pointCount > 0);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref bodyB._xf, radiusB);
ContactConstraint cc = _constraints[i];
cc.bodyA = bodyA;
cc.bodyB = bodyB;
cc.manifold = manifold;
cc.normal = worldManifold._normal;
cc.pointCount = manifold._pointCount;
cc.friction = friction;
cc.localNormal = manifold._localNormal;
cc.localPoint = manifold._localPoint;
cc.radius = radiusA + radiusB;
cc.type = manifold._type;
for (int j = 0; j < cc.pointCount; ++j)
{
ManifoldPoint cp = manifold._points[j];
ContactConstraintPoint ccp = cc.points[j];
ccp.normalImpulse = impulseRatio * cp.NormalImpulse;
ccp.tangentImpulse = impulseRatio * cp.TangentImpulse;
ccp.localPoint = cp.LocalPoint;
ccp.rA = worldManifold._points[j] - bodyA._sweep.c;
ccp.rB = worldManifold._points[j] - bodyB._sweep.c;
#if MATH_OVERLOADS
float rnA = MathUtils.Cross(ccp.rA, cc.normal);
float rnB = MathUtils.Cross(ccp.rB, cc.normal);
#else
float rnA = ccp.rA.x * cc.normal.y - ccp.rA.y * cc.normal.x;
float rnB = ccp.rB.x * cc.normal.y - ccp.rB.y * cc.normal.x;
#endif
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
//Debug.Assert(kNormal > Settings.b2_epsilon);
ccp.normalMass = 1.0f / kNormal;
#if MATH_OVERLOADS
Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f);
float rtA = MathUtils.Cross(ccp.rA, tangent);
float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
Vector2 tangent = new Vector2(cc.normal.y, -cc.normal.x);
float rtA = ccp.rA.x * tangent.y - ccp.rA.y * tangent.x;
float rtB = ccp.rB.x * tangent.y - ccp.rB.y * tangent.x;
#endif
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
//Debug.Assert(kTangent > Settings.b2_epsilon);
ccp.tangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.velocityBias = 0.0f;
float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA));
if (vRel < -Settings.b2_velocityThreshold)
{
ccp.velocityBias = -restitution * vRel;
}
cc.points[j] = ccp;
}
// If we have two points, then prepare the block solver.
if (cc.pointCount == 2)
{
ContactConstraintPoint ccp1 = cc.points[0];
ContactConstraintPoint ccp2 = cc.points[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = MathUtils.Cross(ccp1.rA, cc.normal);
float rn1B = MathUtils.Cross(ccp1.rB, cc.normal);
float rn2A = MathUtils.Cross(ccp2.rA, cc.normal);
float rn2B = MathUtils.Cross(ccp2.rB, cc.normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
cc.normalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.pointCount = 1;
}
}
_constraints[i] = cc;
}
}
public ContactSolver(ContactSolverDef def)
{
m_step = def.step;
m_positionConstraints = new List <ContactPositionConstraint>();
m_velocityConstraints = new List <ContactVelocityConstraint>();
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < def.contacts.Count(); ++i)
{
Contact contact = m_contacts[i];
Fixture fixtureA = contact.m_fixtureA;
Fixture fixtureB = contact.m_fixtureB;
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold = contact.GetManifold();
int pointCount = manifold.points.Count();
Utilities.Assert(pointCount > 0);
ContactVelocityConstraint vc = new ContactVelocityConstraint();
vc.friction = contact.m_friction;
vc.restitution = contact.m_restitution;
vc.tangentSpeed = contact.m_tangentSpeed;
vc.indexA = bodyA.m_islandIndex;
vc.indexB = bodyB.m_islandIndex;
vc.invMassA = bodyA.m_invMass;
vc.invMassB = bodyB.m_invMass;
vc.invIA = bodyA.m_invI;
vc.invIB = bodyB.m_invI;
vc.contactIndex = i;
//vc.points.Count() = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
ContactPositionConstraint pc = new ContactPositionConstraint();
pc.indexA = bodyA.m_islandIndex;
pc.indexB = bodyB.m_islandIndex;
pc.invMassA = bodyA.m_invMass;
pc.invMassB = bodyB.m_invMass;
pc.localCenterA = bodyA.m_sweep.localCenter;
pc.localCenterB = bodyB.m_sweep.localCenter;
pc.invIA = bodyA.m_invI;
pc.invIB = bodyB.m_invI;
pc.localNormal = manifold.localNormal;
pc.localPoint = manifold.localPoint;
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
for (int j = 0; j < pointCount; ++j)
{
ManifoldPoint cp = manifold.points[j];
VelocityConstraintPoint vcp = new VelocityConstraintPoint();
if (m_step.warmStarting)
{
vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse;
}
else
{
vcp.normalImpulse = 0.0f;
vcp.tangentImpulse = 0.0f;
}
vcp.rA.SetZero();
vcp.rB.SetZero();
vcp.normalMass = 0.0f;
vcp.tangentMass = 0.0f;
vcp.velocityBias = 0.0f;
vc.points.Add(vcp);
pc.localPoints[j] = cp.localPoint;
}
m_velocityConstraints.Add(vc);
m_positionConstraints.Add(pc);
}
}
