public void FinalizeVelocityConstraints()
{
for (int i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint c = m_constraints[i];
b2Manifold m = c.manifold;
for (int j = 0; j < c.pointCount; ++j)
{
b2ManifoldPoint point1 = m.m_points[j];
b2ContactConstraintPoint point2 = c.points[j];
point1.m_normalImpulse = point2.normalImpulse;
point1.m_tangentImpulse = point2.tangentImpulse;
}
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2ManifoldPoint obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
// 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);
}
}
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;
}
}
public void Initialize(b2TimeStep step, List <b2Contact> contacts, int contactCount, object allocator)
{
b2Contact contact;
m_step.Set(step);
m_allocator = allocator;
int i;
b2Vec2 tVec;
b2Mat22 tMat;
m_constraintCount = contactCount;
// fill vector to hold enough constraints
while (m_constraints.Count < m_constraintCount)
{
m_constraints.Add(new b2ContactConstraint());
}
for (i = 0; i < contactCount; ++i)
{
contact = contacts[i];
b2Fixture fixtureA = contact.m_fixtureA;
b2Fixture fixtureB = contact.m_fixtureB;
b2Shape shapeA = fixtureA.m_shape;
b2Shape shapeB = fixtureB.m_shape;
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
b2Body bodyA = fixtureA.m_body;
b2Body bodyB = fixtureB.m_body;
b2Manifold manifold = contact.GetManifold();
float friction = b2Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());;
float restitution = b2Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
//var vA:b2Vec2 = bodyA.m_linearVelocity.Copy();
float vAX = bodyA.m_linearVelocity.x;
float vAY = bodyA.m_linearVelocity.y;
//var vB:b2Vec2 = bodyB.m_linearVelocity.Copy();
float vBX = bodyB.m_linearVelocity.x;
float vBY = bodyB.m_linearVelocity.y;
float wA = bodyA.m_angularVelocity;
float wB = bodyB.m_angularVelocity;
b2Settings.b2Assert(manifold.m_pointCount > 0);
s_worldManifold.Initialize(manifold, bodyA.m_xf, radiusA, bodyB.m_xf, radiusB);
float normalX = s_worldManifold.m_normal.x;
float normalY = s_worldManifold.m_normal.y;
b2ContactConstraint cc = m_constraints[i];
cc.bodyA = bodyA; //p
cc.bodyB = bodyB; //p
cc.manifold = manifold; //p
//c.normal = normal;
cc.normal.x = normalX;
cc.normal.y = normalY;
cc.pointCount = manifold.m_pointCount;
cc.friction = friction;
//-----------------------------修改 2015/12/10 13:07 by kingBook------------------
float bevel = 10.0f;
float planeAngle;
int vx;
if (!bodyA.m_allowBevelSlither || bodyA.m_uphillZeroFriction)
{
planeAngle = Mathf.Atan2(normalY, normalX) * 57.3f + 90.0f;
vx = (int)(bodyA.m_linearVelocity.x);
if (planeAngle < 0.0f)
{
planeAngle += 360.0f;
}
if ((planeAngle > bevel && planeAngle < 90.0f - bevel) || (planeAngle > 180.0f + bevel && planeAngle < 270.0f - bevel))//斜面 左上角-右下角
{
if (!bodyA.m_allowBevelSlither)
{
if (vx >= 0.0f)
{
cc.friction = 1.0f;
}
}
if (bodyA.m_uphillZeroFriction)
{
if (vx < 0.0f)
{
cc.friction = 0.0f;
}
}
}
else if ((planeAngle > 90.0f && planeAngle < 180.0f - bevel) || (planeAngle > 270.0f + bevel && planeAngle < 360.0f - bevel))//斜面 左下角-右上角
{
if (!bodyA.m_allowBevelSlither)
{
if (vx <= 0.0f)
{
cc.friction = 1.0f;
}
}
if (bodyA.m_uphillZeroFriction)
{
if (vx > 0.0f)
{
cc.friction = 0.0f;
}
}
}
}
else if (!bodyB.m_allowBevelSlither || bodyB.m_uphillZeroFriction)
{
planeAngle = Mathf.Atan2(-normalY, -normalX) * 57.3f + 90.0f;
vx = (int)(bodyB.m_linearVelocity.x);
if (planeAngle < 0.0f)
{
planeAngle += 360.0f;
}
if ((planeAngle > bevel && planeAngle < 90.0f - bevel) || (planeAngle > 180.0f + bevel && planeAngle < 270.0f - bevel))//斜面 左上角-右下角
{
if (!bodyB.m_allowBevelSlither)
{
if (vx >= 0.0f)
{
cc.friction = 1.0f;
}
}
if (bodyB.m_uphillZeroFriction)
{
if (vx < 0.0f)
{
cc.friction = 0.0f;
}
}
}
else if ((planeAngle > 90.0f && planeAngle < 180.0f - bevel) || (planeAngle > 270.0f + bevel && planeAngle < 360.0f - bevel))//斜面 左下角-右上角
{
if (!bodyB.m_allowBevelSlither)
{
if (vx <= 0.0f)
{
cc.friction = 1.0f;
}
}
if (bodyB.m_uphillZeroFriction)
{
if (vx > 0)
{
cc.friction = 0.0f;
}
}
}
}
if (bodyA.m_isIgnoreFrictionX || bodyB.m_isIgnoreFrictionX)
{
if (Mathf.Abs(normalY) > 0.9f)
{
cc.friction = 0.0f;
}
}
else if (bodyA.m_isIgnoreFrictionY || bodyB.m_isIgnoreFrictionY)
{
if (Mathf.Abs(normalX) > 0.9f)
{
cc.friction = 0.0f;
}
}
//-----------------------------修改结束------------------
cc.restitution = restitution;
cc.localPlaneNormal.x = manifold.m_localPlaneNormal.x;
cc.localPlaneNormal.y = manifold.m_localPlaneNormal.y;
cc.localPoint.x = manifold.m_localPoint.x;
cc.localPoint.y = manifold.m_localPoint.y;
cc.radius = radiusA + radiusB;
cc.type = manifold.m_type;
for (int k = 0; k < cc.pointCount; ++k)
{
b2ManifoldPoint cp = manifold.m_points[k];
b2ContactConstraintPoint ccp = cc.points[k];
ccp.normalImpulse = cp.m_normalImpulse;
ccp.tangentImpulse = cp.m_tangentImpulse;
ccp.localPoint.SetV(cp.m_localPoint);
float rAX = ccp.rA.x = s_worldManifold.m_points[k].x - bodyA.m_sweep.c.x;
float rAY = ccp.rA.y = s_worldManifold.m_points[k].y - bodyA.m_sweep.c.y;
float rBX = ccp.rB.x = s_worldManifold.m_points[k].x - bodyB.m_sweep.c.x;
float rBY = ccp.rB.y = s_worldManifold.m_points[k].y - bodyB.m_sweep.c.y;
float rnA = rAX * normalY - rAY * normalX; //b2Math.b2Cross(r1, normal);
float rnB = rBX * normalY - rBY * normalX; //b2Math.b2Cross(r2, normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA.m_invMass + bodyB.m_invMass + bodyA.m_invI * rnA + bodyB.m_invI * rnB;
//b2Settings.b2Assert(kNormal > Number.MIN_VALUE);
ccp.normalMass = 1.0f / kNormal;
float kEqualized = bodyA.m_mass * bodyA.m_invMass + bodyB.m_mass * bodyB.m_invMass;
kEqualized += bodyA.m_mass * bodyA.m_invI * rnA + bodyB.m_mass * bodyB.m_invI * rnB;
//b2Assert(kEqualized > Number.MIN_VALUE);
ccp.equalizedMass = 1.0f / kEqualized;
//var tangent:b2Vec2 = b2Math.b2CrossVF(normal, 1.0);
float tangentX = normalY;
float tangentY = -normalX;
//var rtA:Number = b2Math.b2Cross(rA, tangent);
float rtA = rAX * tangentY - rAY * tangentX;
//var rtB:Number = b2Math.b2Cross(rB, tangent);
float rtB = rBX * tangentY - rBY * tangentX;
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA.m_invMass + bodyB.m_invMass + bodyA.m_invI * rtA + bodyB.m_invI * rtB;
//b2Settings.b2Assert(kTangent > Number.MIN_VALUE);
ccp.tangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.velocityBias = 0.0f;
//b2Dot(c.normal, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
float tX = vBX + (-wB * rBY) - vAX - (-wA * rAY);
float tY = vBY + (wB * rBX) - vAY - (wA * rAX);
//var vRel:Number = b2Dot(cc.normal, t);
float vRel = cc.normal.x * tX + cc.normal.y * tY;
if (vRel < -b2Settings.b2_velocityThreshold)
{
ccp.velocityBias += -cc.restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.pointCount == 2)
{
b2ContactConstraintPoint ccp1 = cc.points[0];
b2ContactConstraintPoint ccp2 = cc.points[1];
float invMassA = bodyA.m_invMass;
float invIA = bodyA.m_invI;
float invMassB = bodyB.m_invMass;
float invIB = bodyB.m_invI;
//var rn1A:Number = b2Cross(ccp1.rA, normal);
//var rn1B:Number = b2Cross(ccp1.rB, normal);
//var rn2A:Number = b2Cross(ccp2.rA, normal);
//var rn2B:Number = b2Cross(ccp2.rB, normal);
float rn1A = ccp1.rA.x * normalY - ccp1.rA.y * normalX;
float rn1B = ccp1.rB.x * normalY - ccp1.rB.y * normalX;
float rn2A = ccp2.rA.x * normalY - ccp2.rA.y * normalX;
float rn2B = ccp2.rB.x * normalY - ccp2.rB.y * normalX;
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.
float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.col1.Set(k11, k12);
cc.K.col2.Set(k12, k22);
cc.K.GetInverse(cc.normalMass);
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.pointCount = 1;
}
}
}
//b2Settings.b2Assert(count == m_constraintCount);
}
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;
}
}
}
// 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;
}
// 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);
}
}
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);
}
}
