public override void PreSolve(Contact contact, Manifold oldManifold)
{
Manifold manifold = contact.Manifold;
if (manifold.PointCount == 0)
{
return;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
PointState[] state1 = new PointState[Box2DSettings.b2_maxManifoldPoints],
state2 = new PointState[Box2DSettings.b2_maxManifoldPoints];
Manifold.GetPointStates(ref state1, ref state2, ref oldManifold, ref manifold);
WorldManifold worldManifold = contact.WorldManifold;
for (int i = 0; i < manifold.PointCount && m_pointCount < ContactPoint.k_maxContactPoints; ++i)
{
m_points[m_pointCount].fixtureA = fixtureA;
m_points[m_pointCount].fixtureB = fixtureB;
m_points[m_pointCount].position = worldManifold.Points[i];
m_points[m_pointCount].normal = worldManifold.Normal;
m_points[m_pointCount].state = state2[i];
++m_pointCount;
}
}
public override void Step(Framework.Settings settings)
{
Manifold manifold = new Manifold();
Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref _transformA, _polygonA._radius, ref _transformB, _polygonB._radius);
_debugDraw.DrawString(50, _textLine, "point count = {0:n}", manifold._pointCount);
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
FixedArray8 <Vector2> v = new FixedArray8 <Vector2>();
for (int i = 0; i < _polygonA._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color);
for (int i = 0; i < _polygonB._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color);
}
for (int i = 0; i < manifold._pointCount; ++i)
{
_debugDraw.DrawPoint(worldManifold._points[i], 0.5f, new Color(0.9f, 0.3f, 0.3f));
}
}
public override void Step(TestSettings settings)
{
Manifold manifold;
Collision.CollidePolygons(out manifold, m_polygonA, m_transformA, m_polygonB, m_transformB);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, m_transformA, m_polygonA.m_radius, m_transformB, m_polygonB.m_radius);
m_debugDraw.DrawString("point count = {0}", manifold.points.Count());
{
Color color = Color.FromArgb(225, 225, 225);
Vec2[] v = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < m_polygonA.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);
for (int i = 0; i < m_polygonB.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
}
for (int i = 0; i < manifold.points.Count(); ++i)
{
m_debugDraw.DrawPoint(worldManifold.points[i], 4.0f, Color.FromArgb(225, 75, 75));
}
}
public override void OnRender()
{
var manifold = new Manifold();
CollisionUtils.CollidePolygons(ref manifold, _polygonA, _transformA, _polygonB, _transformB);
var worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, _transformA, _polygonA.Radius, _transformB, _polygonB.Radius);
DrawString($"point count = {manifold.PointCount}");
{
var color = Color.FromArgb(230, 230, 230);
var v = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _polygonA.Count; ++i)
{
v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonA.Count, color);
for (var i = 0; i < _polygonB.Count; ++i)
{
v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonB.Count, color);
}
for (var i = 0; i < manifold.PointCount; ++i)
{
Drawer.DrawPoint(worldManifold.Points[i], 4.0f, Color.FromArgb(230, 77, 77));
}
}
public override void Update(GameSettings settings, GameTime gameTime)
{
Manifold manifold = new Manifold();
CollidePolygon.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB);
WorldManifold.Initialize(ref manifold, ref _transformA, _polygonA.Radius, ref _transformB, _polygonB.Radius, out _, out FixedArray2 <Vector2> points, out _);
DrawString("Point count = " + manifold.PointCount);
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
{
Color color = new Color(0.9f, 0.9f, 0.9f);
Vector2[] v = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < _polygonA.Vertices.Count; ++i)
{
v[i] = MathUtils.Mul(ref _transformA, _polygonA.Vertices[i]);
}
DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color);
for (int i = 0; i < _polygonB.Vertices.Count; ++i)
{
v[i] = MathUtils.Mul(ref _transformB, _polygonB.Vertices[i]);
}
DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color);
}
for (int i = 0; i < manifold.PointCount; ++i)
{
DebugView.DrawPoint(points[i], 0.1f, new Color(0.9f, 0.3f, 0.3f));
DrawString(points[i].ToString());
}
DebugView.EndCustomDraw();
}
public override void Step(Framework.Settings settings)
{
Manifold manifold = new Manifold();
Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref _transformA, _polygonA._radius, ref _transformB, _polygonB._radius);
_debugDraw.DrawString(50, _textLine, "point count = {0:n}", manifold._pointCount);
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
FixedArray8<Vector2> v = new FixedArray8<Vector2>();
for (int i = 0; i < _polygonA._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color);
for (int i = 0; i < _polygonB._vertexCount; ++i)
{
v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]);
}
_debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color);
}
for (int i = 0; i < manifold._pointCount; ++i)
{
_debugDraw.DrawPoint(worldManifold._points[i], 0.5f, new Color(0.9f, 0.3f, 0.3f));
}
}
/// <summary>
/// Get the world manifold.
/// </summary>
public void GetWorldManifold(WorldManifold worldManifold)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
worldManifold.Initialize(Manifold, bodyA.GetTransform(), shapeA.Radius, bodyB.GetTransform(), shapeB.Radius);
}
/// <summary>
/// Gets the world manifold.
/// </summary>
/// <param name="worldManifold">The world manifold.</param>
public void GetWorldManifold(out WorldManifold worldManifold)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
worldManifold = new WorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius);
}
/**
* Get the world manifold.
*/
public void getWorldManifold(WorldManifold worldManifold)
{
Body bodyA = m_fixtureA.getBody();
Body bodyB = m_fixtureB.getBody();
Shape shapeA = m_fixtureA.getShape();
Shape shapeB = m_fixtureB.getShape();
worldManifold.initialize(m_manifold, bodyA.getTransform(), shapeA.m_radius,
bodyB.getTransform(), shapeB.m_radius);
}
/// <summary>
/// Get the world manifold.
/// </summary>
public void GetWorldManifold(out WorldManifold worldManifold)
{
worldManifold = new WorldManifold();
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
worldManifold.Initialize(Manifold, bodyA.GetXForm(), shapeA.Radius, bodyB.GetXForm(), shapeB.Radius);
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2 <Vector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
FixedArray2 <float> _;
WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points, out _);
}
/// <summary>
/// Get the world manifold.
/// </summary>
public void GetWorldManifold(out WorldManifold worldManifold)
{
worldManifold = new WorldManifold();
Body bodyA = _fixtureA.Body;
Body bodyB = _fixtureB.Body;
Shape shapeA = _fixtureA.Shape;
Shape shapeB = _fixtureB.Shape;
worldManifold.Initialize(_manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius);
}
public void BeginContact(Contact contact)
{
GameObject A = (GameObject)contact.GetFixtureA().GetUserData();
GameObject B = (GameObject)contact.GetFixtureB().GetUserData();
Manifold manifold;
Transform xfA;
Transform xfB;
float radiusA;
float radiusB;
contact.GetFixtureA().GetBody().GetTransform(out xfA);
contact.GetFixtureB().GetBody().GetTransform(out xfB);
radiusA = contact.GetFixtureA().GetShape()._radius;
radiusB = contact.GetFixtureB().GetShape()._radius;
contact.GetManifold(out manifold);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref xfA, radiusA, ref xfB, radiusB);
Vector2 ptA = worldManifold._points[0];
Vector2 ptB = worldManifold._points[1];
//System.Console.Write(" point {0} {1}\n", ptA, ptB);
EvenMessage e = new EvenMessage(A.gameId, B.gameId, ptA);
if (A.type == GameObjType.p1Bomb || A.type == GameObjType.p1missiles || A.type == GameObjType.p1ship)
{
OutQueue.add(e, PlayerID.one);
}
else if (A.type == GameObjType.p2Bomb || A.type == GameObjType.p2missiles || A.type == GameObjType.p2ship)
{
OutQueue.add(e, PlayerID.two);
}
else if (B.type == GameObjType.p1Bomb || B.type == GameObjType.p1missiles || B.type == GameObjType.p1ship)
{
OutQueue.add(e, PlayerID.one);
}
else if (B.type == GameObjType.p2Bomb || B.type == GameObjType.p2missiles || B.type == GameObjType.p2ship)
{
OutQueue.add(e, PlayerID.two);
}
else
{
OutQueue.add(e, PlayerID.one);
}
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = contactCount;
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
for (int i = 0; i < _constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
Box2DNetDebug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB);
ContactConstraint cc = _constraints[i];
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.Radius = radiusA + radiusB;
cc.Type = manifold.Type;
ContactSolverSetup(manifold, worldManifold, cc);
}
}
/// <summary>
/// Gets the world manifold.
/// </summary>
/// <param name="worldManifold">The world manifold.</param>
public void GetWorldManifold(out WorldManifold worldManifold)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
Transform xfA, xfB;
bodyA.GetTransform(out xfA);
bodyB.GetTransform(out xfB);
worldManifold = new WorldManifold(ref Manifold, ref xfA, shapeA.Radius, ref xfB, shapeB.Radius);
}
/// Get the world manifold.
public void GetWorldManifold(out WorldManifold worldManifold)
{
var bodyA = FixtureA.Body;
var bodyB = FixtureB.Body;
var shapeA = FixtureA.Shape;
var shapeB = FixtureB.Shape;
worldManifold = new WorldManifold();
worldManifold.Initialize(
Manifold,
bodyA.GetTransform(),
shapeA.Radius,
bodyB.GetTransform(),
shapeB.Radius);
}
public void Reset(Contact[] contacts, int contactCount, float impulseRatio)
{
_contacts = contacts;
_constraintCount = contactCount;
// grow the array
if (Constraints == null || Constraints.Length < _constraintCount)
{
Constraints = new ContactConstraint[_constraintCount * 2];
for (int i = 0; i < _constraintCount * 2; i++)
{
Constraints[i] = new ContactConstraint();
}
}
for (int i = 0; i < _constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold;
contact.GetManifold(out manifold);
float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
Vector2 vA = bodyA.LinearVelocityInternal;
Vector2 vB = bodyB.LinearVelocityInternal;
float wA = bodyA.AngularVelocityInternal;
float wB = bodyB.AngularVelocityInternal;
Debug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf,
radiusB);
ContactConstraint cc = Constraints[i];
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.LocalNormal = manifold.LocalNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.Radius = radiusA + radiusB;
cc.Type = manifold.Type;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = cc.Points[j];
ccp.NormalImpulse = impulseRatio * cp.NormalImpulse;
ccp.TangentImpulse = impulseRatio * cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.rA = worldManifold.Points[j] - bodyA.Sweep.c;
ccp.rB = worldManifold.Points[j] - bodyB.Sweep.c;
#if MATH_OVERLOADS
float rnA = MathUtils.Cross(ccp.rA, cc.Normal);
float rnB = MathUtils.Cross(ccp.rB, cc.Normal);
#else
float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X;
float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X;
#endif
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB;
Debug.Assert(kNormal > Settings.Epsilon);
ccp.NormalMass = 1.0f / kNormal;
#if MATH_OVERLOADS
Vector2 tangent = MathUtils.Cross(cc.Normal, 1.0f);
float rtA = MathUtils.Cross(ccp.rA, tangent);
float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X);
float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
#endif
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB;
Debug.Assert(kTangent > Settings.Epsilon);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal,
vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMassA = bodyA.InvMass;
float invIA = bodyA.InvI;
float invMassB = bodyB.InvMass;
float invIB = bodyB.InvI;
float rn1A = MathUtils.Cross(ccp1.rA, cc.Normal);
float rn1B = MathUtils.Cross(ccp1.rB, cc.Normal);
float rn2A = MathUtils.Cross(ccp2.rA, cc.Normal);
float rn2B = MathUtils.Cross(ccp2.rB, cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
cc.NormalMass = cc.K.Inverse;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
if (fixtureA.PostSolve != null)
{
fixtureA.PostSolve(cc);
}
if (fixtureB.PostSolve != null)
{
fixtureB.PostSolve(cc);
}
}
}
public void InitializeVelocityConstraints()
{
for (int i = 0; i < _count; ++i)
{
ContactVelocityConstraint vc = _velocityConstraints[i];
ContactPositionConstraint pc = _positionConstraints[i];
float radiusA = pc.radiusA;
float radiusB = pc.radiusB;
Manifold manifold = _contacts[vc.contactIndex].Manifold;
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
float mB = vc.invMassB;
float iA = vc.invIA;
float iB = vc.invIB;
Vector2 localCenterA = pc.localCenterA;
Vector2 localCenterB = pc.localCenterB;
Vector2 cA = _positions[indexA].c;
float aA = _positions[indexA].a;
Vector2 vA = _velocities[indexA].v;
float wA = _velocities[indexA].w;
Vector2 cB = _positions[indexB].c;
float aB = _positions[indexB].a;
Vector2 vB = _velocities[indexB].v;
float wB = _velocities[indexB].w;
Debug.Assert(manifold.PointCount > 0);
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
FixedArray2 <Vector2> points;
WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points);
vc.normal = normal;
Vector2 tangent = MathUtils.Rot270(ref vc.normal);
int pointCount = vc.pointCount;
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = vc.points[j];
vcp.rA = points[j] - cA;
vcp.rB = points[j] - cB;
float rnA = MathUtils.Cross(ref vcp.rA, ref vc.normal);
float rnB = MathUtils.Cross(ref vcp.rB, ref vc.normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
float rtA = MathUtils.Cross(ref vcp.rA, ref tangent);
float rtB = MathUtils.Cross(ref vcp.rB, ref 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 = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA));
if (vRel < -Settings.VelocityThreshold)
{
vcp.velocityBias = -vc.restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.pointCount == 2)
{
VelocityConstraintPoint vcp1 = vc.points[0];
VelocityConstraintPoint vcp2 = vc.points[1];
float rn1A = MathUtils.Cross(ref vcp1.rA, ref vc.normal);
float rn1B = MathUtils.Cross(ref vcp1.rB, ref vc.normal);
float rn2A = MathUtils.Cross(ref vcp2.rA, ref vc.normal);
float rn2B = MathUtils.Cross(ref vcp2.rB, ref 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 = new Vector2(k11, k12);
vc.K.ey = new Vector2(k12, k22);
vc.normalMass = vc.K.Inverse;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.pointCount = 1;
}
}
}
}
/**
* Get the world manifold.
*/
public void getWorldManifold(WorldManifold worldManifold)
{
Body bodyA = m_fixtureA.getBody();
Body bodyB = m_fixtureB.getBody();
Shape shapeA = m_fixtureA.getShape();
Shape shapeB = m_fixtureB.getShape();
worldManifold.initialize(m_manifold, bodyA.getTransform(), shapeA.m_radius,
bodyB.getTransform(), shapeB.m_radius);
}
internal void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc)
{
// this is kind of yucky but we do know these were setup before entry to this method
var bodyA = cc.BodyA;
var bodyB = cc.BodyB;
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
ContactConstraintPoint[] ccPointsPtr = cc.Points;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = ccPointsPtr[j];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = ccp.RA.Cross(cc.Normal);
float rnB = ccp.RB.Cross(cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
ccp.EqualizedMass = 1.0f / kEqualized;
Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f);
float rtA = ccp.RA.Cross(tangent);
float rtB = ccp.RB.Cross(tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal, vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA));
if (vRel < -Common.Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = ccPointsPtr[0];
ContactConstraintPoint ccp2 = ccPointsPtr[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = ccp1.RA.Cross(cc.Normal);
float rn1B = ccp1.RB.Cross(cc.Normal);
float rn2A = ccp2.RA.Cross(cc.Normal);
float rn2B = ccp2.RB.Cross(cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1 = new Vector2(k11, k12);
cc.K.Col2 = new Vector2(k12, k22);
cc.NormalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
public void InitializeVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint cc = Constraints[i];
float radiusA = cc.RadiusA;
float radiusB = cc.RadiusB;
Body bodyA = cc.BodyA;
Body bodyB = cc.BodyB;
Manifold manifold = cc.Manifold;
Vector2 vA = bodyA.LinearVelocity;
Vector2 vB = bodyB.LinearVelocity;
float wA = bodyA.AngularVelocity;
float wB = bodyB.AngularVelocity;
Debug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB);
cc.Normal = worldManifold.Normal;
for (int j = 0; j < cc.PointCount; ++j)
{
ContactConstraintPoint ccp = cc.Points[j];
ccp.rA = worldManifold.Points[j] - bodyA.Sweep.C;
ccp.rB = worldManifold.Points[j] - bodyB.Sweep.C;
#if MATH_OVERLOADS
float rnA = MathUtils.Cross(ccp.rA, cc.Normal);
float rnB = MathUtils.Cross(ccp.rB, cc.Normal);
#else
float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X;
float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X;
#endif
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB;
Debug.Assert(kNormal > Settings.Epsilon);
ccp.NormalMass = 1.0f / kNormal;
#if MATH_OVERLOADS
Vector2 tangent = MathUtils.Cross(cc.Normal, 1.0f);
float rtA = MathUtils.Cross(ccp.rA, tangent);
float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X);
float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
#endif
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB;
Debug.Assert(kTangent > Settings.Epsilon);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal, vB + new Vector2(-wB * ccp.rB.Y, wB * ccp.rB.X) - vA - new Vector2(-wA * ccp.rA.Y, wA * ccp.rA.X));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMassA = bodyA.InvMass;
float invIA = bodyA.InvI;
float invMassB = bodyB.InvMass;
float invIB = bodyB.InvI;
float rn1A = MathUtils.Cross(ccp1.rA, cc.Normal);
float rn1B = MathUtils.Cross(ccp1.rB, cc.Normal);
float rn2A = MathUtils.Cross(ccp2.rA, cc.Normal);
float rn2B = MathUtils.Cross(ccp2.rB, cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
cc.NormalMass = cc.K.Inverse;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
/// <inheritdoc />
protected override void PostStep()
{
if (Input.GetKeyDown(KeyCode.A))
{
_positionB.X -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.D))
{
_positionB.X += 0.1f;
}
if (Input.GetKeyDown(KeyCode.S))
{
_positionB.Y -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.W))
{
_positionB.Y += 0.1f;
}
if (Input.GetKeyDown(KeyCode.Q))
{
_angleB += 0.1f * Settings.Pi;
}
if (Input.GetKeyDown(KeyCode.E))
{
_angleB -= 0.1f * Settings.Pi;
}
_transformB.Set(_positionB, _angleB);
var manifold = new Manifold();
manifold.Initialize();
CollisionUtils.CollidePolygons(ref manifold, _polygonA, _transformA, _polygonB, _transformB);
var worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, _transformA, _polygonA.Radius, _transformB, _polygonB.Radius);
DrawString($"point count = {manifold.PointCount}");
{
var color = Color.FromArgb(230, 230, 230);
var v = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _polygonA.Count; ++i)
{
v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonA.Count, color);
for (var i = 0; i < _polygonB.Count; ++i)
{
v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonB.Count, color);
}
for (var i = 0; i < manifold.PointCount; ++i)
{
Drawer.DrawPoint(worldManifold.Points[i], 4.0f, Color.FromArgb(230, 77, 77));
}
}
public void InitializeVelocityConstraints()
{
for (int i = 0; i < _count; ++i)
{
ContactVelocityConstraint vc = _velocityConstraints[i];
ContactPositionConstraint pc = _positionConstraints[i];
float radiusA = pc.radiusA;
float radiusB = pc.radiusB;
Manifold manifold = _contacts[vc.contactIndex].Manifold;
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
float mB = vc.invMassB;
float iA = vc.invIA;
float iB = vc.invIB;
Vector2 localCenterA = pc.localCenterA;
Vector2 localCenterB = pc.localCenterB;
Vector2 cA = _positions[indexA].c;
float aA = _positions[indexA].a;
Vector2 vA = _velocities[indexA].v;
float wA = _velocities[indexA].w;
Vector2 cB = _positions[indexB].c;
float aB = _positions[indexB].a;
Vector2 vB = _velocities[indexB].v;
float wB = _velocities[indexB].w;
//Debug.Assert(manifold.pointCount > 0);
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q = Matrex.CreateRotation(aA); // Actually about twice as fast to use our own function
xfB.q = Matrex.CreateRotation(aB); // Actually about twice as fast to use our own function
xfA.p = cA - Vector2.Transform(localCenterA, xfA.q); // Common.Math.Mul(xfA.q, localCenterA);
xfB.p = cB - Vector2.Transform(localCenterB, xfB.q); // Common.Math.Mul(xfB.q, localCenterB);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.normal = worldManifold.normal;
int pointCount = vc.pointCount;
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = vc.points[j];
vcp.rA = worldManifold.points[j] - cA;
vcp.rB = worldManifold.points[j] - cB;
float rnA = Vectex.Cross(vcp.rA, vc.normal);
float rnB = Vectex.Cross(vcp.rB, vc.normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.normalMass = kNormal > 0f ? 1f / kNormal : 0f;
Vector2 tangent = Vectex.Cross(vc.normal, 1f);
float rtA = Vectex.Cross(vcp.rA, tangent);
float rtB = Vectex.Cross(vcp.rB, tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.tangentMass = kTangent > 0f ? 1f / kTangent : 0f;
vcp.velocityBias = 0f;
float vRel = Vector2.Dot(vc.normal, vB + Vectex.Cross(wB, vcp.rB) - vA - Vectex.Cross(wA, vcp.rA));
if (vRel < -Settings.VelocityThreshold)
{
vcp.velocityBias = -vc.restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.pointCount == 2 && Settings.BlockSolve)
{
VelocityConstraintPoint vcp1 = vc.points[0];
VelocityConstraintPoint vcp2 = vc.points[1];
float rn1A = Vectex.Cross(vcp1.rA, vc.normal);
float rn1B = Vectex.Cross(vcp1.rB, vc.normal);
float rn2A = Vectex.Cross(vcp2.rA, vc.normal);
float rn2B = Vectex.Cross(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 = new Matrix3x2(k11, k12, k12, k22, 0, 0);
// vc.K.ex = new Vector2(k11, k12);
// vc.K.ey = new Vector2(k12, k22);
/*Matrix3x2*/
Matrex.Invert(vc.K, out Matrix3x2 KT);
vc.normalMass = KT;
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.pointCount = 1;
}
}
}
}
public void BeginContact(Contact contact)
{
GameObject A = (GameObject)contact.GetFixtureA().GetUserData();
GameObject B = (GameObject)contact.GetFixtureB().GetUserData();
Manifold manifold;
Transform xfA;
Transform xfB;
float radiusA;
float radiusB;
contact.GetFixtureA().GetBody().GetTransform(out xfA);
contact.GetFixtureB().GetBody().GetTransform(out xfB);
radiusA = contact.GetFixtureA().GetShape()._radius;
radiusB = contact.GetFixtureB().GetShape()._radius;
contact.GetManifold(out manifold);
WorldManifold worldManifold = new WorldManifold(ref manifold, ref xfA, radiusA, ref xfB, radiusB);
Vector2 ptA = worldManifold._points[0];
Vector2 ptB = worldManifold._points[1];
// adding this for networking
if (A.GameID == 1)
{
int x = 0;
}
System.Console.Write("v--> sending mc point A:{0} B:{1} {2} {3}\n", A.GameID, B.GameID, ptA, ptB);
Debug.Assert(A != null);
Debug.Assert(B != null);
Event_Message msg = new Event_Message(A.GameID, B.GameID, ptA);
OutputQueue.add(msg);
//System.Console.Write(" point {0} {1}\n", ptA, ptB);
// if (A.CollideAvailable == true && B.CollideAvailable == true)
{/*
* if (A.type < B.type)
* {
* A.Accept(B, ptA);
* }
* else
* {
* B.Accept(A, ptA);
* }*/
}
//if (A.type == GameObjType.p1missiles || A.type == GameObjType.p2missiles)
//{
// A.CollideAvailable = false;
//}
//if (B.type == GameObjType.p1missiles || B.type == GameObjType.p2missiles)
//{
// B.CollideAvailable = false;
//}
}
public void InitializeVelocityConstraints()
{
for (var i = 0; i < _count; ++i)
{
var vc = VelocityConstraints[i];
var pc = _positionConstraints[i];
var radiusA = pc.RadiusA;
var radiusB = pc.RadiusB;
var manifold = _contacts[vc.ContactIndex].GetManifold();
var indexA = vc.IndexA;
var indexB = vc.IndexB;
var mA = vc.InvMassA;
var mB = vc.InvMassB;
var iA = vc.InvIa;
var iB = vc.InvIb;
var localCenterA = pc.LocalCenterA;
var localCenterB = pc.LocalCenterB;
var cA = _positions[indexA].Center;
var aA = _positions[indexA].Angle;
var vA = _velocities[indexA].V;
var wA = _velocities[indexA].W;
var cB = _positions[indexB].Center;
var aB = _positions[indexB].Angle;
var vB = _velocities[indexB].V;
var wB = _velocities[indexB].W;
Debug.Assert(manifold.PointCount > 0);
var xfA = new Transform();
var xfB = new Transform();
xfA.Rotation.Set(aA);
xfB.Rotation.Set(aB);
xfA.Position = cA - MathUtils.Mul(xfA.Rotation, localCenterA);
xfB.Position = cB - MathUtils.Mul(xfB.Rotation, localCenterB);
var worldManifold = WorldManifold.Create();
worldManifold.Initialize(
manifold,
xfA,
radiusA,
xfB,
radiusB);
vc.Normal = worldManifold.normal;
var pointCount = vc.PointCount;
for (var j = 0; j < pointCount; ++j)
{
var vcp = vc.Points[j];
vcp.Ra = worldManifold.points[j] - cA;
vcp.Rb = worldManifold.points[j] - cB;
var rnA = MathUtils.Cross(vcp.Ra, vc.Normal);
var rnB = MathUtils.Cross(vcp.Rb, vc.Normal);
var kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
var tangent = MathUtils.Cross(vc.Normal, 1.0f);
var rtA = MathUtils.Cross(vcp.Ra, tangent);
var rtB = MathUtils.Cross(vcp.Rb, tangent);
var 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;
var vRel = MathUtils.Dot(
vc.Normal,
vB + MathUtils.Cross(wB, vcp.Rb) - vA - MathUtils.Cross(wA, vcp.Ra));
if (vRel < -Settings.VelocityThreshold)
{
vcp.VelocityBias = -vc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.PointCount == 2 && BlockSolve)
{
var vcp1 = vc.Points[0];
var vcp2 = vc.Points[1];
var rn1A = MathUtils.Cross(vcp1.Ra, vc.Normal);
var rn1B = MathUtils.Cross(vcp1.Rb, vc.Normal);
var rn2A = MathUtils.Cross(vcp2.Ra, vc.Normal);
var rn2B = MathUtils.Cross(vcp2.Rb, vc.Normal);
var k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
var k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
var k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float maxConditionNumber = 1000.0f;
if (k11 * k11 < 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 ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = contactCount;
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
int count = 0;
for (int i = 0; i < _constraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
Vec2 vA = bodyA._linearVelocity;
Vec2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
Box2DXDebug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB);
ContactConstraint cc = _constraints[i];
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.Radius = radiusA + radiusB;
cc.Type = manifold.Type;
unsafe
{
fixed(ContactConstraintPoint *ccPointsPtr = cc.Points)
{
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint *ccp = &ccPointsPtr[j];
ccp->NormalImpulse = cp.NormalImpulse;
ccp->TangentImpulse = cp.TangentImpulse;
ccp->LocalPoint = cp.LocalPoint;
ccp->RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp->RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = Vec2.Cross(ccp->RA, cc.Normal);
float rnB = Vec2.Cross(ccp->RB, cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp->NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp->EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);
float rtA = Vec2.Cross(ccp->RA, tangent);
float rtB = Vec2.Cross(ccp->RB, tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp->TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp->VelocityBias = 0.0f;
float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA));
if (vRel < -Common.Settings.VelocityThreshold)
{
ccp->VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint *ccp1 = &ccPointsPtr[0];
ContactConstraintPoint *ccp2 = &ccPointsPtr[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = Vec2.Cross(ccp1->RA, cc.Normal);
float rn1B = Vec2.Cross(ccp1->RB, cc.Normal);
float rn2A = Vec2.Cross(ccp2->RA, cc.Normal);
float rn2B = Vec2.Cross(ccp2->RB, cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1.Set(k11, k12);
cc.K.Col2.Set(k12, k22);
cc.NormalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
}
}