// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public void Collide(b2Manifold manifold, b2EdgeShape edgeA, b2Transform xfA, b2PolygonShape polygonB, b2Transform xfB)
{
m_xf = Utils.b2MulT(xfA, xfB);
m_centroidB = Utils.b2Mul(m_xf, polygonB.m_centroid);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
bool hasVertex0 = edgeA.m_hasVertex0;
bool hasVertex3 = edgeA.m_hasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
float offset1 = Utils.b2Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f;
float offset2 = 0.0f;
bool convex1 = false;
bool convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = Utils.b2Cross(edge0, edge1) >= 0.0f;
offset0 = Utils.b2Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = Utils.b2Cross(edge1, edge2) > 0.0f;
offset2 = Utils.b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
m_polygonB.vertices[i] = Utils.b2Mul(m_xf, polygonB.m_vertices[i]);
m_polygonB.normals[i] = Utils.b2Mul(m_xf.q, polygonB.m_normals[i]);
}
m_radius = polygonB.m_radius + edgeA.m_radius;
manifold.pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxis.Type.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxis.Type.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2EPAxis primaryAxis = new b2EPAxis();
if (polygonAxis.type == b2EPAxis.Type.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex[] ie = Arrays.InitializeWithDefaultInstances <b2ClipVertex>(2);
b2ReferenceFace rf = new b2ReferenceFace();
if (primaryAxis.type == b2EPAxis.Type.e_edgeA)
{
manifold.type = b2Manifold.Type.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Utils.b2Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = Utils.b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)i1;
ie[0].id.cf.typeA = (int)b2ContactFeature.Type.e_face;
ie[0].id.cf.typeB = (int)b2ContactFeature.Type.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)i2;
ie[1].id.cf.typeA = (int)b2ContactFeature.Type.e_face;
ie[1].id.cf.typeB = (int)b2ContactFeature.Type.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = b2Manifold.Type.e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)primaryAxis.index;
ie[0].id.cf.typeA = (int)b2ContactFeature.Type.e_vertex;
ie[0].id.cf.typeB = (int)b2ContactFeature.Type.e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)primaryAxis.index;
ie[1].id.cf.typeA = (int)b2ContactFeature.Type.e_vertex;
ie[1].id.cf.typeB = (int)b2ContactFeature.Type.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Utils.b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Utils.b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = Arrays.InitializeWithDefaultInstances <b2ClipVertex>(2);
b2ClipVertex[] clipPoints2 = Arrays.InitializeWithDefaultInstances <b2ClipVertex>(2);
int np;
// Clip to box side 1
np = Utils.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Utils.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis.Type.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.m_normals[rf.i1];
manifold.localPoint = polygonB.m_vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = Utils.b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == b2EPAxis.Type.e_edgeA)
{
cp.localPoint = Utils.b2MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public b2ContactSolver(b2ContactSolverDef def)
{
m_step = def.step;
m_count = def.count;
m_positionConstraints = new b2ContactPositionConstraint[m_count];
for (int pc = 0; pc < m_count; pc++)
{
m_positionConstraints[pc] = b2ContactPositionConstraint.Create();
}
m_velocityConstraints = new b2ContactVelocityConstraint[m_count];
for (int vc = 0; vc < m_count; vc++)
{
m_velocityConstraints[vc] = b2ContactVelocityConstraint.Create();
}
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < m_count; ++i)
{
b2Contact contact = m_contacts[i];
b2Fixture fixtureA = contact.FixtureA;
b2Fixture fixtureB = contact.FixtureB;
b2Shape shapeA = fixtureA.Shape;
b2Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
b2Body bodyA = fixtureA.Body;
b2Body bodyB = fixtureB.Body;
b2Manifold manifold = contact.GetManifold();
int pointCount = manifold.pointCount;
Debug.Assert(pointCount > 0);
b2ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.Friction;
vc.restitution = contact.Restitution;
vc.indexA = bodyA.IslandIndex;
vc.indexB = bodyB.IslandIndex;
vc.invMassA = bodyA.InvertedMass;
vc.invMassB = bodyB.InvertedMass;
vc.invIA = bodyA.InvertedI;
vc.invIB = bodyB.InvertedI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
b2ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.IslandIndex;
pc.indexB = bodyB.IslandIndex;
pc.invMassA = bodyA.InvertedMass;
pc.invMassB = bodyB.InvertedMass;
pc.localCenterA = bodyA.Sweep.localCenter;
pc.localCenterB = bodyB.Sweep.localCenter;
pc.invIA = bodyA.InvertedI;
pc.invIB = bodyB.InvertedI;
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)
{
b2ManifoldPoint cp = manifold.points[j];
b2VelocityConstraintPoint vcp = vc.points[j];
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;
pc.localPoints[j] = cp.localPoint;
vc.points[j] = vcp;
}
//Put back the struct data since struct data is copied by value
m_positionConstraints[i] = pc;
m_velocityConstraints[i] = vc;
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Manifold obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB)
{
b2Collision.b2CollideCircles(ref manifold,
(b2CircleShape)m_fixtureA.Shape, ref xfA,
(b2CircleShape)m_fixtureB.Shape, ref xfB);
}
public virtual void SetManifold(b2Manifold m)
{
m_manifold = m;
}
// 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);
}
}
/// <summary> /// This is called after a contact is updated. This allows you to inspect a /// contact before it goes to the solver. If you are careful, you can modify the /// contact manifold (e.g. disable contact). /// A copy of the old manifold is provided so that you can detect changes. /// Note: this is called only for awake bodies. /// Note: this is called even when the number of contact points is zero. /// Note: this is not called for sensors. /// Note: if you set the number of contact points to zero, you will not /// get an EndContact callback. However, you may get a BeginContact callback /// the next step. /// </summary> public abstract void PreSolve(b2Contact contact, b2Manifold oldManifold);
/// <summary>
/// This is called after a contact is updated. This allows you to inspect a
/// contact before it goes to the solver. If you are careful, you can modify the
/// contact manifold (e.g. disable contact).
/// A copy of the old manifold is provided so that you can detect changes.
/// Note: this is called only for awake bodies.
/// Note: this is called even when the number of contact points is zero.
/// Note: this is not called for sensors.
/// Note: if you set the number of contact points to zero, you will not
/// get an EndContact callback. However, you may get a BeginContact callback
/// the next step.
/// </summary>
public virtual void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
}
private void b2CollideEdgeAndCircle(b2Manifold manifold,
b2EdgeShape edge,
b2Transform xf1,
b2CircleShape circle,
b2Transform xf2)
{
//TODO_BORIS
/*
* manifold.m_pointCount = 0;
* var tMat: b2Mat22;
* var tVec: b2Vec2;
* var dX: Number;
* var dY: Number;
* var tX: Number;
* var tY: Number;
* var tPoint:b2ManifoldPoint;
*
* //b2Vec2 c = b2Mul(xf2, circle->GetLocalPosition());
* tMat = xf2.R;
* tVec = circle.m_r;
* var cX: Number = xf2.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
* var cY: Number = xf2.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
*
* //b2Vec2 cLocal = b2MulT(xf1, c);
* tMat = xf1.R;
* tX = cX - xf1.position.x;
* tY = cY - xf1.position.y;
* var cLocalX: Number = (tX * tMat.col1.x + tY * tMat.col1.y );
* var cLocalY: Number = (tX * tMat.col2.x + tY * tMat.col2.y );
*
* var n: b2Vec2 = edge.m_normal;
* var v1: b2Vec2 = edge.m_v1;
* var v2: b2Vec2 = edge.m_v2;
* var radius: Number = circle.m_radius;
* var separation: Number;
*
* var dirDist: Number = (cLocalX - v1.x) * edge.m_direction.x +
* (cLocalY - v1.y) * edge.m_direction.y;
*
* var normalCalculated: Boolean = false;
*
* if (dirDist <= 0) {
* dX = cLocalX - v1.x;
* dY = cLocalY - v1.y;
* if (dX * edge.m_cornerDir1.x + dY * edge.m_cornerDir1.y < 0) {
* return;
* }
* dX = cX - (xf1.position.x + (tMat.col1.x * v1.x + tMat.col2.x * v1.y));
* dY = cY - (xf1.position.y + (tMat.col1.y * v1.x + tMat.col2.y * v1.y));
* } else if (dirDist >= edge.m_length) {
* dX = cLocalX - v2.x;
* dY = cLocalY - v2.y;
* if (dX * edge.m_cornerDir2.x + dY * edge.m_cornerDir2.y > 0) {
* return;
* }
* dX = cX - (xf1.position.x + (tMat.col1.x * v2.x + tMat.col2.x * v2.y));
* dY = cY - (xf1.position.y + (tMat.col1.y * v2.x + tMat.col2.y * v2.y));
* } else {
* separation = (cLocalX - v1.x) * n.x + (cLocalY - v1.y) * n.y;
* if (separation > radius || separation < -radius) {
* return;
* }
* separation -= radius;
*
* //manifold.normal = b2Mul(xf1.R, n);
* tMat = xf1.R;
* manifold.normal.x = (tMat.col1.x * n.x + tMat.col2.x * n.y);
* manifold.normal.y = (tMat.col1.y * n.x + tMat.col2.y * n.y);
*
* normalCalculated = true;
* }
*
* if (!normalCalculated) {
* var distSqr: Number = dX * dX + dY * dY;
* if (distSqr > radius * radius)
* {
* return;
* }
*
* if (distSqr < Number.MIN_VALUE)
* {
* separation = -radius;
* manifold.normal.x = (tMat.col1.x * n.x + tMat.col2.x * n.y);
* manifold.normal.y = (tMat.col1.y * n.x + tMat.col2.y * n.y);
* }
* else
* {
* distSqr = Math.sqrt(distSqr);
* dX /= distSqr;
* dY /= distSqr;
* separation = distSqr - radius;
* manifold.normal.x = dX;
* manifold.normal.y = dY;
* }
* }
*
* tPoint = manifold.points[0];
* manifold.pointCount = 1;
* tPoint.id.key = 0;
* tPoint.separation = separation;
* cX = cX - radius * manifold.normal.x;
* cY = cY - radius * manifold.normal.y;
*
* tX = cX - xf1.position.x;
* tY = cY - xf1.position.y;
* tPoint.localPoint1.x = (tX * tMat.col1.x + tY * tMat.col1.y );
* tPoint.localPoint1.y = (tX * tMat.col2.x + tY * tMat.col2.y );
*
* tMat = xf2.R;
* tX = cX - xf2.position.x;
* tY = cY - xf2.position.y;
* tPoint.localPoint2.x = (tX * tMat.col1.x + tY * tMat.col1.y );
* tPoint.localPoint2.y = (tX * tMat.col2.x + tY * tMat.col2.y );
*/
}
public void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
}
public void Initialize(b2Manifold manifold, b2Transform xfA, float radiusA, b2Transform xfB, float radiusB) {
Box2dPINVOKE.b2WorldManifold_Initialize(swigCPtr, b2Manifold.getCPtr(manifold), b2Transform.getCPtr(xfA), radiusA, b2Transform.getCPtr(xfB), radiusB);
if (Box2dPINVOKE.SWIGPendingException.Pending) throw Box2dPINVOKE.SWIGPendingException.Retrieve();
}
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);
}
}
// Initialize position dependent portions of the velocity constraints.
public void InitializeVelocityConstraints()
{
for (int i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint vc = m_velocityConstraints[i];
b2ContactPositionConstraint pc = m_positionConstraints[i];
float radiusA = pc.radiusA;
float radiusB = pc.radiusB;
b2Manifold manifold = m_contacts[vc.contactIndex].GetManifold();
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
float mB = vc.invMassB;
float iA = vc.invIA;
float iB = vc.invIB;
b2Vec2 localCenterA = new b2Vec2(pc.localCenterA);
b2Vec2 localCenterB = new b2Vec2(pc.localCenterB);
b2Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
b2Vec2 vA = m_velocities[indexA].v;
float wA = m_velocities[indexA].w;
b2Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
b2Vec2 vB = m_velocities[indexB].v;
float wB = m_velocities[indexB].w;
Debug.Assert(manifold.pointCount > 0);
b2Transform xfA = new b2Transform();
b2Transform xfB = new b2Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Utils.b2Mul(xfA.q, localCenterA);
xfB.p = cB - Utils.b2Mul(xfB.q, localCenterB);
b2WorldManifold worldManifold = new b2WorldManifold();
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.normal = worldManifold.normal;
int pointCount = vc.pointCount;
for (int j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint vcp = vc.points[j];
vcp.rA = worldManifold.points[j] - cA;
vcp.rB = worldManifold.points[j] - cB;
float rnA = Utils.b2Cross(vcp.rA, vc.normal);
float rnB = Utils.b2Cross(vcp.rB, vc.normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
b2Vec2 tangent = Utils.b2Cross(vc.normal, 1.0f);
float rtA = Utils.b2Cross(vcp.rA, tangent);
float rtB = Utils.b2Cross(vcp.rB, tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp.velocityBias = 0.0f;
float vRel = Utils.b2Dot(vc.normal, vB + Utils.b2Cross(wB, vcp.rB) - vA - Utils.b2Cross(wA, vcp.rA));
if (vRel < -Settings.b2_velocityThreshold)
{
vcp.velocityBias = -vc.restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.pointCount == 2 && Utils.g_blockSolve)
{
b2VelocityConstraintPoint vcp1 = vc.points[0];
b2VelocityConstraintPoint vcp2 = vc.points[1];
float rn1A = Utils.b2Cross(vcp1.rA, vc.normal);
float rn1B = Utils.b2Cross(vcp1.rB, vc.normal);
float rn2A = Utils.b2Cross(vcp2.rA, vc.normal);
float rn2B = Utils.b2Cross(vcp2.rB, vc.normal);
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 1000.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc.K.ex.Set(k11, k12);
vc.K.ey.Set(k12, k22);
vc.normalMass = vc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.pointCount = 1;
}
}
}
}
public virtual void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
Box2DPINVOKE.b2ContactListener_PreSolve(swigCPtr, b2Contact.getCPtr(contact), b2Manifold.getCPtr(oldManifold));
}
public b2ContactSolver(b2ContactSolverDef def)
{
m_step = def.step;
m_count = def.count;
m_positionConstraints = Arrays.InitializeWithDefaultInstances <b2ContactPositionConstraint>(m_count);
m_velocityConstraints = Arrays.InitializeWithDefaultInstances <b2ContactVelocityConstraint>(m_count);
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < m_count; ++i)
{
b2Contact contact = m_contacts[i];
b2Fixture fixtureA = contact.m_fixtureA;
b2Fixture fixtureB = contact.m_fixtureB;
b2Shape shapeA = fixtureA.GetShape();
b2Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
b2Manifold manifold = contact.GetManifold();
int pointCount = manifold.pointCount;
Debug.Assert(pointCount > 0);
b2ContactVelocityConstraint vc = m_velocityConstraints[i];
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.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
b2ContactPositionConstraint pc = m_positionConstraints[i];
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)
{
b2ManifoldPoint cp = manifold.points[j];
b2VelocityConstraintPoint vcp = vc.points[j];
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;
pc.localPoints[j] = cp.localPoint;
}
}
}
/// Evaluate this contact with your own manifold and transforms. public abstract void Evaluate(b2Manifold manifold, b2Transform xfA, b2Transform xfB);
public override void Evaluate(b2Manifold manifold, b2Transform xfA, b2Transform xfB)
{
Utils.b2CollideCircles(manifold, (b2CircleShape)m_fixtureA.GetShape(), xfA, (b2CircleShape)m_fixtureB.GetShape(), xfB);
}
private void b2CollidePolyAndEdge(b2Manifold manifold,
b2PolygonShape polygon,
b2Transform xf1,
b2EdgeShape edge,
b2Transform xf2)
{
//TODO_BORIS
/*
* manifold.pointCount = 0;
* var tMat: b2Mat22;
* var tVec1: b2Vec2;
* var tVec2: b2Vec2;
* var tX: Number;
* var tY: Number;
* var tPoint:b2ManifoldPoint;
* var ratio: Number;
*
* //b2Vec2 v1 = b2Mul(xf2, edge->GetVertex1());
* tMat = xf2.R;
* tVec1 = edge.m_v1;
* var v1X: Number = xf2.position.x + (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y);
* var v1Y: Number = xf2.position.y + (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y);
*
* //b2Vec2 v2 = b2Mul(xf2, edge->GetVertex2());
* tVec1 = edge.m_v2;
* var v2X: Number = xf2.position.x + (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y);
* var v2Y: Number = xf2.position.y + (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y);
*
* //b2Vec2 n = b2Mul(xf2.R, edge->GetNormalVector());
* tVec1 = edge.m_normal;
* var nX: Number = (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y);
* var nY: Number = (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y);
*
* //b2Vec2 v1Local = b2MulT(xf1, v1);
* tMat = xf1.R;
* tX = v1X - xf1.position.x;
* tY = v1Y - xf1.position.y;
* var v1LocalX: Number = (tX * tMat.col1.x + tY * tMat.col1.y );
* var v1LocalY: Number = (tX * tMat.col2.x + tY * tMat.col2.y );
*
* //b2Vec2 v2Local = b2MulT(xf1, v2);
* tX = v2X - xf1.position.x;
* tY = v2Y - xf1.position.y;
* var v2LocalX: Number = (tX * tMat.col1.x + tY * tMat.col1.y );
* var v2LocalY: Number = (tX * tMat.col2.x + tY * tMat.col2.y );
*
* //b2Vec2 nLocal = b2MulT(xf1.R, n);
* var nLocalX: Number = (nX * tMat.col1.x + nY * tMat.col1.y );
* var nLocalY: Number = (nX * tMat.col2.x + nY * tMat.col2.y );
*
* var separation1: Number;
* var separationIndex1: int = -1; // which normal on the poly found the shallowest depth?
* var separationMax1: Number = -Number.MAX_VALUE; // the shallowest depth of edge in poly
* var separation2: Number;
* var separationIndex2: int = -1; // which normal on the poly found the shallowest depth?
* var separationMax2: Number = -Number.MAX_VALUE; // the shallowest depth of edge in poly
* var separationMax: Number = -Number.MAX_VALUE; // the shallowest depth of edge in poly
* var separationV1: Boolean = false; // is the shallowest depth from edge's v1 or v2 vertex?
* var separationIndex: int = -1; // which normal on the poly found the shallowest depth?
*
* var vertexCount: int = polygon.m_vertexCount;
* var vertices: Array = polygon.m_vertices;
* var normals: Array = polygon.m_normals;
*
* var enterStartIndex: int = -1; // the last poly vertex above the edge
* var enterEndIndex: int = -1; // the first poly vertex below the edge
* var exitStartIndex: int = -1; // the last poly vertex below the edge
* var exitEndIndex: int = -1; // the first poly vertex above the edge
*
* // the "N" in the following variables refers to the edge's normal.
* // these are projections of poly vertices along the edge's normal,
* // a.k.a. they are the separation of the poly from the edge.
* var prevSepN: Number = 0.0;
* var nextSepN: Number = 0.0;
* var enterSepN: Number = 0.0; // the depth of enterEndIndex under the edge (stored as a separation, so it's negative)
* var exitSepN: Number = 0.0; // the depth of exitStartIndex under the edge (stored as a separation, so it's negative)
* var deepestSepN: Number = Number.MAX_VALUE; // the depth of the deepest poly vertex under the end (stored as a separation, so it's negative)
*
*
* // for each poly normal, get the edge's depth into the poly.
* // for each poly vertex, get the vertex's depth into the edge.
* // use these calculations to define the remaining variables declared above.
* tVec1 = vertices[vertexCount-1];
* prevSepN = (tVec1.x - v1LocalX) * nLocalX + (tVec1.y - v1LocalY) * nLocalY;
* for (var i: int = 0; i < vertexCount; i++)
* {
* tVec1 = vertices[i];
* tVec2 = normals[i];
* separation1 = (v1LocalX - tVec1.x) * tVec2.x + (v1LocalY - tVec1.y) * tVec2.y;
* separation2 = (v2LocalX - tVec1.x) * tVec2.x + (v2LocalY - tVec1.y) * tVec2.y;
* if (separation2 < separation1) {
* if (separation2 > separationMax) {
* separationMax = separation2;
* separationV1 = false;
* separationIndex = i;
* }
* } else {
* if (separation1 > separationMax) {
* separationMax = separation1;
* separationV1 = true;
* separationIndex = i;
* }
* }
* if (separation1 > separationMax1) {
* separationMax1 = separation1;
* separationIndex1 = i;
* }
* if (separation2 > separationMax2) {
* separationMax2 = separation2;
* separationIndex2 = i;
* }
*
* nextSepN = (tVec1.x - v1LocalX) * nLocalX + (tVec1.y - v1LocalY) * nLocalY;
* if (nextSepN >= 0.0 && prevSepN < 0.0) {
* exitStartIndex = (i == 0) ? vertexCount-1 : i-1;
* exitEndIndex = i;
* exitSepN = prevSepN;
* } else if (nextSepN < 0.0 && prevSepN >= 0.0) {
* enterStartIndex = (i == 0) ? vertexCount-1 : i-1;
* enterEndIndex = i;
* enterSepN = nextSepN;
* }
* if (nextSepN < deepestSepN) {
* deepestSepN = nextSepN;
* }
* prevSepN = nextSepN;
* }
*
* if (enterStartIndex == -1) {
* // poly is entirely below or entirely above edge, return with no contact:
* return;
* }
* if (separationMax > 0.0) {
* // poly is laterally disjoint with edge, return with no contact:
* return;
* }
*
* // if the poly is near a convex corner on the edge
* if ((separationV1 && edge.m_cornerConvex1) || (!separationV1 && edge.m_cornerConvex2)) {
* // if shallowest depth was from edge into poly,
* // use the edge's vertex as the contact point:
* if (separationMax > deepestSepN + b2Settings.b2_linearSlop) {
* // if -normal angle is closer to adjacent edge than this edge,
* // let the adjacent edge handle it and return with no contact:
* if (separationV1) {
* tMat = xf2.R;
* tVec1 = edge.m_cornerDir1;
* tX = (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y);
* tY = (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y);
* tMat = xf1.R;
* v1X = (tMat.col1.x * tX + tMat.col2.x * tY); // note abuse of v1...
* v1Y = (tMat.col1.y * tX + tMat.col2.y * tY);
* tVec2 = normals[separationIndex1];
* if (tVec2.x * v1X + tVec2.y * v1Y >= 0.0) {
* return;
* }
* } else {
* tMat = xf2.R;
* tVec1 = edge.m_cornerDir2;
* tX = (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y);
* tY = (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y);
* tMat = xf1.R;
* v1X = (tMat.col1.x * tX + tMat.col2.x * tY); // note abuse of v1...
* v1Y = (tMat.col1.y * tX + tMat.col2.y * tY);
* tVec2 = normals[separationIndex2];
* if (tVec2.x * v1X + tVec2.y * v1Y <= 0.0) {
* return;
* }
* }
*
* tPoint = manifold.points[0];
* manifold.pointCount = 1;
*
* //manifold->normal = b2Mul(xf1.R, normals[separationIndex]);
* tMat = xf1.R;
* tVec2 = normals[separationIndex];
* manifold.normal.x = (tMat.col1.x * tVec2.x + tMat.col2.x * tVec2.y);
* manifold.normal.y = (tMat.col1.y * tVec2.x + tMat.col2.y * tVec2.y);
*
* tPoint.separation = separationMax;
* tPoint.id.features.incidentEdge = separationIndex;
* tPoint.id.features.incidentVertex = b2Collision.b2_nullFeature;
* tPoint.id.features.referenceEdge = 0;
* tPoint.id.features.flip = 0;
* if (separationV1) {
* tPoint.localPoint1.x = v1LocalX;
* tPoint.localPoint1.y = v1LocalY;
* tPoint.localPoint2.x = edge.m_v1.x;
* tPoint.localPoint2.y = edge.m_v1.y;
* } else {
* tPoint.localPoint1.x = v2LocalX;
* tPoint.localPoint1.y = v2LocalY;
* tPoint.localPoint2.x = edge.m_v2.x;
* tPoint.localPoint2.y = edge.m_v2.y;
* }
* return;
* }
* }
*
* // We're going to use the edge's normal now.
* manifold.normal.x = -nX;
* manifold.normal.y = -nY;
*
* // Check whether we only need one contact point.
* if (enterEndIndex == exitStartIndex) {
* tPoint = manifold.points[0];
* manifold.pointCount = 1;
* tPoint.id.features.incidentEdge = enterEndIndex;
* tPoint.id.features.incidentVertex = b2Collision.b2_nullFeature;
* tPoint.id.features.referenceEdge = 0;
* tPoint.id.features.flip = 0;
* tVec1 = vertices[enterEndIndex];
* tPoint.localPoint1.x = tVec1.x;
* tPoint.localPoint1.y = tVec1.y;
*
* tMat = xf1.R;
* tX = xf1.position.x + (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y) - xf2.position.x;
* tY = xf1.position.y + (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y) - xf2.position.y;
* tMat = xf2.R;
* tPoint.localPoint2.x = (tX * tMat.col1.x + tY * tMat.col1.y );
* tPoint.localPoint2.y = (tX * tMat.col2.x + tY * tMat.col2.y );
*
* tPoint.separation = enterSepN;
* return;
* }
*
* manifold.pointCount = 2;
*
* // the edge's direction vector, but in the frame of the polygon:
* tX = -nLocalY;
* tY = nLocalX;
*
* tVec1 = vertices[enterEndIndex];
* var dirProj1: Number = tX * (tVec1.x - v1LocalX) + tY * (tVec1.y - v1LocalY);
* var dirProj2: Number;
*
* // The contact resolution is more robust if the two manifold points are
* // adjacent to each other on the polygon. So pick the first two poly
* // vertices that are under the edge:
* exitEndIndex = (enterEndIndex == vertexCount - 1) ? 0 : enterEndIndex + 1;
* tVec1 = vertices[exitStartIndex];
* if (exitEndIndex != exitStartIndex) {
* exitStartIndex = exitEndIndex;
*
* exitSepN = nLocalX * (tVec1.x - v1LocalX) + nLocalY * (tVec1.y - v1LocalY);
* }
* dirProj2 = tX * (tVec1.x - v1LocalX) + tY * (tVec1.y - v1LocalY);
*
* tPoint = manifold.points[0];
* tPoint.id.features.incidentEdge = enterEndIndex;
* tPoint.id.features.incidentVertex = b2Collision.b2_nullFeature;
* tPoint.id.features.referenceEdge = 0;
* tPoint.id.features.flip = 0;
*
* if (dirProj1 > edge.m_length) {
* tPoint.localPoint1.x = v2LocalX;
* tPoint.localPoint1.y = v2LocalY;
* tPoint.localPoint2.x = edge.m_v2.x;
* tPoint.localPoint2.y = edge.m_v2.y;
* ratio = (edge.m_length - dirProj2) / (dirProj1 - dirProj2);
* if (ratio > 100.0 * Number.MIN_VALUE && ratio < 1.0) {
* tPoint.separation = exitSepN * (1.0 - ratio) + enterSepN * ratio;
* } else {
* tPoint.separation = enterSepN;
* }
* } else {
* tVec1 = vertices[enterEndIndex];
* tPoint.localPoint1.x = tVec1.x;
* tPoint.localPoint1.y = tVec1.y;
*
* tMat = xf1.R;
* tX = xf1.position.x + (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y) - xf2.position.x;
* tY = xf1.position.y + (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y) - xf2.position.y;
* tMat = xf2.R;
* tPoint.localPoint2.x = (tX * tMat.col1.x + tY * tMat.col1.y);
* tPoint.localPoint2.y = (tX * tMat.col2.x + tY * tMat.col2.y);
*
* tPoint.separation = enterSepN;
* }
*
* tPoint = manifold.points[1];
* tPoint.id.features.incidentEdge = exitStartIndex;
* tPoint.id.features.incidentVertex = b2Collision.b2_nullFeature;
* tPoint.id.features.referenceEdge = 0;
* tPoint.id.features.flip = 0;
*
* if (dirProj2 < 0.0) {
* tPoint.localPoint1.x = v1LocalX;
* tPoint.localPoint1.y = v1LocalY;
* tPoint.localPoint2.x = edge.m_v1.x;
* tPoint.localPoint2.y = edge.m_v1.y;
* ratio = (-dirProj1) / (dirProj2 - dirProj1);
* if (ratio > 100.0 * Number.MIN_VALUE && ratio < 1.0) {
* tPoint.separation = enterSepN * (1.0 - ratio) + exitSepN * ratio;
* } else {
* tPoint.separation = exitSepN;
* }
* } else {
* tVec1 = vertices[exitStartIndex];
* tPoint.localPoint1.x = tVec1.x;
* tPoint.localPoint1.y = tVec1.y;
*
* tMat = xf1.R;
* tX = xf1.position.x + (tMat.col1.x * tVec1.x + tMat.col2.x * tVec1.y) - xf2.position.x;
* tY = xf1.position.y + (tMat.col1.y * tVec1.x + tMat.col2.y * tVec1.y) - xf2.position.y;
* tMat = xf2.R;
* tPoint.localPoint2.x = (tX * tMat.col1.x + tY * tMat.col1.y);
* tPoint.localPoint2.y = (tX * tMat.col2.x + tY * tMat.col2.y);
*
* tPoint.separation = exitSepN;
* }
*/
}
