public void Create(BroadPhase broadPhase, Body body, Transform xf, FixtureDef def)
{
UserData = def.UserData;
Friction = def.Friction;
Restitution = def.Restitution;
Density = def.Density;
_body = body;
_next = null;
Filter = def.Filter;
_isSensor = def.IsSensor;
_type = def.Type;
// Allocate and initialize the child shape.
switch (_type)
{
case ShapeType.CircleShape:
{
CircleShape circle = new CircleShape();
CircleDef circleDef = (CircleDef)def;
circle._position = circleDef.LocalPosition;
circle._radius = circleDef.Radius;
_shape = circle;
}
break;
case ShapeType.PolygonShape:
{
PolygonShape polygon = new PolygonShape();
PolygonDef polygonDef = (PolygonDef)def;
polygon.Set(polygonDef.Vertices, polygonDef.VertexCount);
_shape = polygon;
}
break;
case ShapeType.EdgeShape:
{
EdgeShape edge = new EdgeShape();
EdgeDef edgeDef = (EdgeDef)def;
edge.Set(edgeDef.Vertex1, edgeDef.Vertex2);
_shape = edge;
}
break;
default:
break;
}
// Create proxy in the broad-phase.
AABB aabb;
_shape.ComputeAABB(out aabb, xf);
bool inRange = broadPhase.InRange(aabb);
// You are creating a shape outside the world box.
if (inRange)
{
_proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
_proxyId = PairManager.NullProxy;
}
}
public void Solve(TimeStep step, Vector2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force = Vector2.Zero;
b._torque = 0.0f;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b._linearVelocity *= Box2DNet.Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Box2DNet.Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Check for large velocities.
Vector2 translation = step.Dt * b._linearVelocity;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation.Normalized();
}
float rotation = step.Dt * b._angularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
}
else
{
b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new Transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 s1 = _ground.GetTransform().position + _groundAnchor1;
Vector2 s2 = _ground.GetTransform().position + _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpperLimit)
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _constant - length1 - _ratio * length2;
linearError = Box2DNetMath.Max(linearError, -C);
C = Box2DNet.Common.Math.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * r1.Cross(P1);
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * r2.Cross(P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpperLimit)
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
float C = _maxLength1 - length1;
linearError = System.Math.Max(linearError, -C);
C = Box2DNet.Common.Math.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vector2 P1 = -impulse * _u1;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * r1.Cross(P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpperLimit)
{
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p2 = b2._sweep.C + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _maxLength2 - length2;
linearError = Box2DNetMath.Max(linearError, -C);
C = Box2DNet.Common.Math.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vector2 P2 = -impulse * _u2;
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * r2.Cross(P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.C + r2 - b1._sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = r1.Cross(_u);
float cr2u = r2.Cross(_u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
_mass = 1.0f / invMass;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// Frequency
float omega = 2.0f * Settings.Pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * _mass * _dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
_bias = C * step.Dt * k * _gamma;
_mass = 1.0f / (invMass + _gamma);
}
if (step.WarmStarting)
{
//Scale the inpulse to support a variable timestep.
_impulse *= step.DtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * r1.Cross(P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * r2.Cross(P);
}
else
{
_impulse = 0.0f;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
Vector2 s1 = _ground.GetTransform().position + _groundAnchor1;
Vector2 s2 = _ground.GetTransform().position + _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _constant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.InactiveLimit;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpperLimit;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.InactiveLimit;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpperLimit;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.InactiveLimit;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpperLimit;
}
// Compute effective mass.
float cr1u1 = r1.Cross(_u1);
float cr2u2 = r2.Cross(_u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.WarmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.DtRatio;
_limitImpulse1 *= step.DtRatio;
_limitImpulse2 *= step.DtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * r1.Cross(P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * r2.Cross(P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
