internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Shape obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
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;
}
}
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
public virtual void Update(b2ContactListener listener)
{
b2Manifold oldManifold = GetManifold();
// Re-enable this contact.
m_flags |= b2ContactFlags.e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & b2ContactFlags.e_touchingFlag) == b2ContactFlags.e_touchingFlag;
bool sensorA = m_fixtureA.IsSensor;
bool sensorB = m_fixtureB.IsSensor;
bool sensor = sensorA || sensorB;
b2Body bodyA = m_fixtureA.Body;
b2Body bodyB = m_fixtureB.Body;
b2Transform xfA = bodyA.Transform;
b2Transform xfB = bodyB.Transform;
// Is this contact a sensor?
if (sensor)
{
b2Shape shapeA = m_fixtureA.Shape;
b2Shape shapeB = m_fixtureB.Shape;
touching = b2Collision.b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
}
else
{
Evaluate(ref m_manifold, ref xfA, ref xfB);
touching = m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.pointCount; ++i)
{
b2ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
b2ContactFeature id2 = mp2.id;
for (int j = 0; j < oldManifold.pointCount; ++j)
{
b2ManifoldPoint mp1 = oldManifold.points[j];
if (mp1.id.key == id2.key)
{
mp2.normalImpulse = mp1.normalImpulse;
mp2.tangentImpulse = mp1.tangentImpulse;
break;
}
}
m_manifold.points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
m_flags |= b2ContactFlags.e_touchingFlag;
}
else
{
m_flags &= ~b2ContactFlags.e_touchingFlag;
}
if (wasTouching == false && touching == true && listener != null)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && listener != null)
{
listener.EndContact(this);
}
if (sensor == false && touching && listener != null)
{
listener.PreSolve(this, ref oldManifold);
}
}
public 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 virtual void Destroy()
{
m_proxies = null;
m_shape = null;
}
public void SetAsPolygon(Vector2[] vertices)
{
shape = Physics2DNative.GetPolygonShape(vertices);
}
public void SetAsBox(float width, float height)
{
shape = Physics2DNative.GetBoxShape(width, height, Vector2.Zero, 0f);
}
public void SetAsCircle(float radius)
{
shape = Physics2DNative.GetCircleShape(radius, Vector2.Zero);
}
public void Set(b2Shape shape, int index)
{
Box2dPINVOKE.b2DistanceProxy_Set__SWIG_0(swigCPtr, b2Shape.getCPtr(shape), index);
}
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);
}
}
