/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(ay, d)
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
// Spring linear constraint
// C = dot(ax, d)
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
// Motor rotational constraint
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
public void Initialize(Body bA, Body bB, Vec2 anchor, Vec2 axis) {
bodyA = bA;
bodyB = bB;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
localAxisA = bodyA.GetLocalVector(axis);
}
/// Initialize the bodies, anchors, and reference angle using a world
/// anchor point.
// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// C = angle2 - angle1 - referenceAngle
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
public void Initialize(Body bA, Body bB, Vec2 anchor) {
bodyA = bA;
bodyB = bB;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
// 1-D constrained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public void Initialize(Body b1, Body b2,
Vec2 anchor1, Vec2 anchor2) {
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
Vec2 d = anchor2 - anchor1;
length = d.Length();
}
/// Initialize the bodies and offsets using the current transforms.
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
public void Initialize(Body bA, Body bB) {
bodyA = bA;
bodyB = bB;
Vec2 xB = bodyB.GetPosition();
linearOffset = bodyA.GetLocalPoint(xB);
float angleA = bodyA.GetAngle();
float angleB = bodyB.GetAngle();
angularOffset = angleB - angleA;
}
/// Create a rigid body given a definition. No reference to the definition
/// is retained.
/// @warning This function is locked during callbacks.
public Body CreateBody(BodyDef def){
Utilities.Assert(IsLocked() == false);
if (IsLocked())
{
return null;
}
Body b = new Body(def, this);
// Add to world doubly linked list.
m_bodyList.Add(b);
return b;
}
protected Joint(JointDef def){
Utilities.Assert(def.bodyA != def.bodyB);
m_type = def.type;
m_prev = null;
m_next = null;
m_bodyA = def.bodyA;
m_bodyB = def.bodyB;
m_index = 0;
m_collideConnected = def.collideConnected;
m_islandFlag = false;
m_userData = def.userData;
m_edgeA = new List<JointEdge>();
m_edgeB = new List<JointEdge>();
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
// Pulley:
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
// C = C0 - (length1 + ratio * length2)
// u1 = (p1 - s1) / norm(p1 - s1)
// u2 = (p2 - s2) / norm(p2 - s2)
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
public void Initialize(Body bA, Body bB,
Vec2 groundA, Vec2 groundB,
Vec2 anchorA, Vec2 anchorB,
float r) {
bodyA = bA;
bodyB = bB;
groundAnchorA = groundA;
groundAnchorB = groundB;
localAnchorA = bodyA.GetLocalPoint(anchorA);
localAnchorB = bodyB.GetLocalPoint(anchorB);
Vec2 dA = anchorA - groundA;
lengthA = dA.Length();
Vec2 dB = anchorB - groundB;
lengthB = dB.Length();
ratio = r;
Utilities.Assert(ratio > Single.Epsilon);
}
public BulletTest()
{
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 0.0f);
Body body = m_world.CreateBody(bd);
EdgeShape edge = new EdgeShape();
edge.Set(new Vec2(-10.0f, 0.0f), new Vec2(10.0f, 0.0f));
edge.Density = 0;
body.CreateFixture(edge);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.2f, 1.0f, new Vec2(0.5f, 1.0f), 0.0f);
shape.Density = 0;
body.CreateFixture(shape);
}
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 4.0f);
PolygonShape box = new PolygonShape();
box.SetAsBox(2.0f, 0.1f);
box.Density = 100;
m_body = m_world.CreateBody(bd);
m_body.CreateFixture(box);
box.SetAsBox(0.25f, 0.25f);
//m_x = RandomFloat(-1.0f, 1.0f);
m_x = 0.20352793f;
bd.Position.Set(m_x, 10.0f);
bd.bullet = true;
m_bullet = m_world.CreateBody(bd);
m_bullet.CreateFixture(box);
m_bullet.SetLinearVelocity(new Vec2(0.0f, -50.0f));
}
}
public Body AddNode(Body parent, Vec2 localAnchor, int depth, float offset, float a)
{
Vec2 h = new Vec2(0.0f, a);
Vec2 p = parent.GetPosition() + localAnchor - h;
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType._dynamicBody;
bodyDef.Position = p;
Body body = m_world.CreateBody(bodyDef);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f * a, a);
shape.Density = 20;
body.CreateFixture(shape);
if (depth == e_depth)
{
return body;
}
shape.SetAsBox(offset, 0.25f * a, new Vec2(0, -a), 0.0f);
body.CreateFixture(shape);
Vec2 a1 = new Vec2(offset, -a);
Vec2 a2 = new Vec2(-offset, -a);
Body body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a);
Body body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a);
RevoluteJointDef jointDef = new RevoluteJointDef();
jointDef.bodyA = body;
jointDef.localAnchorB = h;
jointDef.localAnchorA = a1;
jointDef.bodyB = body1;
m_world.CreateJoint(jointDef);
jointDef.localAnchorA = a2;
jointDef.bodyB = body2;
m_world.CreateJoint(jointDef);
return body;
}
// This is used to prevent connected bodies from colliding.
// It may lie, depending on the collideConnected flag.
internal bool ShouldCollide(Body other){
// At least one body should be dynamic.
if (m_type != BodyType._dynamicBody && other.m_type != BodyType._dynamicBody) {
return false;
}
// Does a joint prevent collision?
foreach(JointEdge jn in m_jointList){
if (jn.other == other) {
if (jn.joint.m_collideConnected == false) {
return false;
}
}
}
return true;
}
public Joint joint; //pointer ///< the joint
public JointEdge(Joint j, Body body) {
this.joint = j;
this.other = body;
}
public void Add(Body body)
{
body.m_islandIndex = m_bodies.Count();
m_bodies.Add(body);
}
internal Fixture(){
m_userData = null;
m_body = null;
m_next = null;
m_proxies = new List<FixtureProxy>();
m_shape = null;
m_Density = 0.0f;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Vector2 vA = bA._linearVelocity;
float wA = bA._angularVelocity;
Vector2 vB = bB._linearVelocity;
float wB = bB._angularVelocity;
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * _maxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * _maxForce;
if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(rB, impulse);
}
bA._linearVelocity = vA;
bA._angularVelocity = wA;
bB._linearVelocity = vB;
bB._angularVelocity = wB;
}
/// Destroy a rigid body given a definition. No reference to the definition
/// is retained. This function is locked during callbacks.
/// @warning This automatically deletes all associated shapes and joints.
/// @warning This function is locked during callbacks.
public void DestroyBody(Body body){
throw new NotImplementedException();
//Utilities.Assert(m_bodyList.Count() > 0);
//Utilities.Assert(IsLocked() == false);
//if (IsLocked())
//{
// return;
//}
//// Delete the attached joints.
//JointEdge* je = b.m_jointList;
//while (je)
//{
// JointEdge* je0 = je;
// je = je.next;
// if (m_destructionListener)
// {
// m_destructionListener.SayGoodbye(je0.joint);
// }
// DestroyJoint(je0.joint);
// b.m_jointList = je;
//}
//b.m_jointList = null;
//// Delete the attached contacts.
//ContactEdge* ce = b.m_contactList;
//while (ce)
//{
// ContactEdge* ce0 = ce;
// ce = ce.next;
// m_contactManager.Destroy(ce0.contact);
//}
//b.m_contactList = null;
//// Delete the attached fixtures. This destroys broad-phase proxies.
//Fixture* f = b.m_fixtureList;
//while (f)
//{
// Fixture* f0 = f;
// f = f.m_next;
// if (m_destructionListener)
// {
// m_destructionListener.SayGoodbye(f0);
// }
// f0.DestroyProxies(&m_contactManager.m_broadPhase);
// f0.Destroy(&m_blockAllocator);
// f0.~Fixture();
// m_blockAllocator.Free(f0, sizeof(Fixture));
// b.m_fixtureList = f;
// b.m_fixtureCount -= 1;
//}
//b.m_fixtureList = null;
//b.m_fixtureCount = 0;
//// Remove world body list.
//if (b.m_prev)
//{
// b.m_prev.m_next = b.m_next;
//}
//if (b.m_next)
//{
// b.m_next.m_prev = b.m_prev;
//}
//if (b == m_bodyList)
//{
// m_bodyList = b.m_next;
//}
//--m_bodyList.Count();
//b.~Body();
//m_blockAllocator.Free(b, sizeof(Body));
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.c + r2 - b1._sweep.c - r1;
// Handle singularity.
float length = _u.magnitude;
if (length < _length)
{
return;
}
if (length > Settings.b2_linearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = new Vector2(0.0f, 0.0f);
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
//Debug.Assert(invMass > Settings.b2_epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * _mass * _dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (step.warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}
public override void Step(TestSettings settings)
{
base.Step(settings);
// We are going to destroy some bodies according to contact
// points. We must buffer the bodies that should be destroyed
// because they may belong to multiple contact points.
const int k_maxNuke = 6;
Body[] nuke = new Body[k_maxNuke];
int nukeCount = 0;
// Traverse the contact results. Destroy bodies that
// are touching heavier bodies.
for (int i = 0; i < m_pointCount; ++i)
{
ContactPoint point = m_points[i];
Body body1 = point.fixtureA.GetBody();
Body body2 = point.fixtureB.GetBody();
float mass1 = body1.GetMass();
float mass2 = body2.GetMass();
if (mass1 > 0.0f && mass2 > 0.0f)
{
if (mass2 > mass1)
{
nuke[nukeCount++] = body1;
}
else
{
nuke[nukeCount++] = body2;
}
if (nukeCount == k_maxNuke)
{
break;
}
}
}
// Sort the nuke array to group duplicates.
throw new NotImplementedException();
//std::sort(nuke, nuke + nukeCount);
// Destroy the bodies, skipping duplicates.
int i2 = 0;
while (i2 < nukeCount)
{
Body b = nuke[i2++];
while (i2 < nukeCount && nuke[i2] == b)
{
++i2;
}
if (b != m_bomb)
{
m_world.DestroyBody(b);
}
}
}
public ApplyForce() {
m_world.SetGravity(new Vec2(0.0f, 0.0f));
const float k_restitution = 0.4f;
Body ground;
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 20.0f);
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
FixtureDef sd = new FixtureDef();
sd.shape = shape;
sd.Density = 0.0f;
sd.restitution = k_restitution;
// Left vertical
shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(-20.0f, 20.0f));
ground.CreateFixture(sd);
// Right vertical
shape.Set(new Vec2(20.0f, -20.0f), new Vec2(20.0f, 20.0f));
ground.CreateFixture(sd);
// Top horizontal
shape.Set(new Vec2(-20.0f, 20.0f), new Vec2(20.0f, 20.0f));
ground.CreateFixture(sd);
// Bottom horizontal
shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(20.0f, -20.0f));
ground.CreateFixture(sd);
}
{
Transform xf1 = new Transform();
xf1.q.Set(0.3524f * (float)Math.PI);
xf1.p = xf1.q.GetXAxis();
Vec2[] vertices = new Vec2[3];
vertices[0] = Utilities.Mul(xf1, new Vec2(-1.0f, 0.0f));
vertices[1] = Utilities.Mul(xf1, new Vec2(1.0f, 0.0f));
vertices[2] = Utilities.Mul(xf1, new Vec2(0.0f, 0.5f));
PolygonShape poly1 = new PolygonShape();
poly1.Set(vertices, 3);
FixtureDef sd1 = new FixtureDef();
sd1.shape = poly1;
sd1.Density = 4.0f;
Transform xf2 = new Transform();
xf2.q.Set(-0.3524f * (float)Math.PI);
xf2.p = -xf2.q.GetXAxis();
vertices[0] = Utilities.Mul(xf2, new Vec2(-1.0f, 0.0f));
vertices[1] = Utilities.Mul(xf2, new Vec2(1.0f, 0.0f));
vertices[2] = Utilities.Mul(xf2, new Vec2(0.0f, 0.5f));
PolygonShape poly2 = new PolygonShape();
poly2.Set(vertices, 3);
FixtureDef sd2 = new FixtureDef();
sd2.shape = poly2;
sd2.Density = 2.0f;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.angularDamping = 5.0f;
bd.linearDamping = 0.1f;
bd.Position.Set(0.0f, 2.0f);
bd.angle = (float)Math.PI;
bd.allowSleep = false;
m_body = m_world.CreateBody(bd);
m_body.CreateFixture(sd1);
m_body.CreateFixture(sd2);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
fd.friction = 0.3f;
for (int i = 0; i < 10; ++i) {
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 5.0f + 1.54f * i);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
float gravity = 10.0f;
float I = body.GetInertia();
float mass = body.GetMass();
// For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
float radius = (float)Math.Sqrt(2.0f * I / mass);
FrictionJointDef jd = new FrictionJointDef();
jd.localAnchorA.SetZero();
jd.localAnchorB.SetZero();
jd.bodyA = ground;
jd.bodyB = body;
jd.collideConnected = true;
jd.maxForce = mass * gravity;
jd.maxTorque = mass * radius * gravity;
m_world.CreateJoint(jd);
}
}
}
// We need separation create/destroy functions from the constructor/destructor because
// the destructor cannot access the allocator (no destructor arguments allowed by C++).
internal void Create(Body body, FixtureDef def){
m_userData = def.UserData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
m_next = null;
m_filter = def.Filter;
m_isSensor = def.IsSensor;
m_shape = def.shape.Clone();
// Reserve proxy space
int childCount = m_shape.GetChildCount();
m_proxies = new List<FixtureProxy>();
m_Density = def.Density;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Mat22 K1 = new Mat22();
K1.col1.x = mA + mB; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = mA + mB;
Mat22 K2 = new Mat22();
K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x;
Mat22 K3 = new Mat22();
K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.GetInverse();
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA._linearVelocity -= mA * P;
bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB._linearVelocity += mB * P;
bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}
// Perform one solver iteration. Returns true if converged.
public bool Solve(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
{
TOIConstraint c = _constraints[i];
Body bodyA = c.bodyA;
Body bodyB = c.bodyB;
float massA = bodyA._mass;
float massB = bodyB._mass;
// Only the TOI body should move.
if (bodyA == _toiBody)
{
massB = 0.0f;
}
else
{
massA = 0.0f;
}
float invMassA = massA * bodyA._invMass;
float invIA = massA * bodyA._invI;
float invMassB = massB * bodyB._invMass;
float invIB = massB * bodyB._invI;
// Solve normal constraints
for (int j = 0; j < c.pointCount; ++j)
{
TOISolverManifold psm = new TOISolverManifold(ref c, j);
Vector2 normal = psm.normal;
Vector2 point = psm.point;
float separation = psm.separation;
Vector2 rA = point - bodyA._sweep.c;
Vector2 rB = point - bodyB._sweep.c;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(rA, normal);
float rnB = MathUtils.Cross(rB, normal);
float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
bodyA._sweep.c -= invMassA * P;
bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
bodyA.SynchronizeTransform();
bodyB._sweep.c += invMassB * P;
bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.b2_linearSlop);
}