public WorldManifold Clone()
{
WorldManifold newManifold = new WorldManifold();
newManifold.Normal = this.Normal;
this.Points.CopyTo(newManifold.Points, 0);
return(newManifold);
}
public WorldManifold Clone()
{
WorldManifold newManifold = new WorldManifold();
newManifold.Normal = this.Normal;
this.Points.CopyTo(newManifold.Points, 0);
return newManifold;
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
Step = step;
ConstraintCount = contactCount;
Constraints = new ContactConstraint[ConstraintCount];
for (int i = 0; i < ConstraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.GetFixtureA();
Fixture fixtureB = contact.GetFixtureB();
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold = contact.GetManifold();
float friction = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
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.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);
ContactConstraint cc = new ContactConstraint();
Constraints[i] = cc;
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;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = cc.Points[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 > 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 > 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 > 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 < -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 = 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.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.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();
}
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);
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;
ContactSolverSetup(manifold, worldManifold, cc);
}
}
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;
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;
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;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
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;
}
}
}
/// Get the world manifold.
public void GetWorldManifold(out WorldManifold worldManifold)
{
worldManifold = new WorldManifold();
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
worldManifold.Initialize(Manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._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);
}
public override void Step(Settings settings)
{
//B2_NOT_USED(settings);
Manifold manifold;
Collision.CollidePolygons(out manifold, _polygonA, _transformA, _polygonB, _transformB);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, _transformA, _polygonA._radius, _transformB, _polygonB._radius);
OpenGLDebugDraw.DrawString(5, _textLine, string.Format("point count = {0}", manifold.PointCount));
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
Vec2[] v = new Vec2[Box2DX.Common.Settings.MaxPolygonVertices];
for (int i = 0; i < _polygonA.VertexCount; ++i)
{
v[i] = Math.Mul(_transformA, _polygonA.Vertices[i]);
}
_debugDraw.DrawPolygon(v, _polygonA.VertexCount, color);
for (int i = 0; i < _polygonB.VertexCount; ++i)
{
v[i] = Math.Mul(_transformB, _polygonB.Vertices[i]);
}
_debugDraw.DrawPolygon(v, _polygonB.VertexCount, color);
}
for (int i = 0; i < manifold.PointCount; ++i)
{
OpenGLDebugDraw.DrawPoint(worldManifold.Points[i], 4.0f, new Color(0.9f, 0.3f, 0.3f));
}
base.Step(settings);
}
