Delphi/CppBuilder

开发平台：

Delphi

UPhysics2D.pas：源码内容

b := def.body2;
s := b.m_shapeList;
while Assigned(s) do
begin
s.RefilterProxy(m_broadPhase, b.m_xf);
s := s.m_next;
end;
end;
if AutoFreeJointDef then
def.Free;
Result := j;
end;
procedure Tb2World.DestroyJoint(j: Tb2Joint);
var
collideConnected: Boolean;
body1, body2, b: Tb2Body;
s: Tb2Shape;
begin
//b2Assert(m_lock == False);
collideConnected := j.m_collideConnected;
// Remove from the doubly linked list.
if Assigned(j.m_prev) then
j.m_prev.m_next := j.m_next;
if Assigned(j.m_next) then
j.m_next.m_prev := j.m_prev;
if j = m_jointList then
m_jointList := j.m_next;
// Disconnect from island graph.
body1 := j.m_body1;
body2 := j.m_body2;
// Wake up connected bodies.
body1.WakeUp;
body2.WakeUp;
// Remove from body 1.
if Assigned(j.m_node1.prev) then
j.m_node1.prev^.next := j.m_node1.next;
if Assigned(j.m_node1.next) then
j.m_node1.next^.prev := j.m_node1.prev;
if (@j.m_node1) = body1.m_jointList then
body1.m_jointList := j.m_node1.next;
j.m_node1.prev := nil;
j.m_node1.next := nil;
// Remove from body 2
if Assigned(j.m_node2.prev) then
j.m_node2.prev^.next := j.m_node2.next;
if Assigned(j.m_node2.next) then
j.m_node2.next^.prev := j.m_node2.prev;
if (@j.m_node2) = body2.m_jointList then
body2.m_jointList := j.m_node2.next;
j.m_node2.prev := nil;
j.m_node2.next := nil;
j.Free;
//b2Assert(m_jointCount > 0);
Dec(m_jointCount);
// If the joint prevents collisions, then reset collision filtering.
if not collideConnected then
begin
// Reset the proxies on the body with the minimum number of shapes.
if body1.m_shapeCount < body2.m_shapeCount then
b := body1
else
b := body2;
s := b.m_shapeList;
while Assigned(s) do
begin
s.RefilterProxy(m_broadPhase, b.m_xf);
s := s.m_next;
end;
end;
end;
procedure Tb2World.Step(timeStep: Float; iterations: Int32; drawThisStep: Boolean = True);
var
step: Tb2TimeStep;
begin
m_lock := True;
step.dt := timeStep;
step.maxIterations := iterations;
if timeStep > 0.0 then
step.inv_dt := 1.0 / timeStep
else
step.inv_dt := 0.0;
step.dtRatio := m_inv_dt0 * timeStep;
step.positionCorrection := m_positionCorrection;
step.warmStarting := m_warmStarting;
// Update contacts.
m_contactManager.Collide;
if step.dt > 0.0 then
begin
Solve(step); // Integrate velocities, solve velocity constraints, and integrate positions.
if m_continuousPhysics then
SolveTOI(step); // Handle TOI events.
end;
if drawThisStep then
DrawDebugData;
m_inv_dt0 := step.inv_dt;
m_lock := False;
end;
function Tb2World.Query(const aabb: Tb2AABB; shapes: TList; maxCount: Int32): Int32;
begin
Result := m_broadPhase.Query(aabb, shapes, maxCount);
end;
procedure Tb2World.Refilter(shape: Tb2Shape);
begin
shape.RefilterProxy(m_broadPhase, shape.GetBody.m_xf);
end;
procedure Tb2World.Validate;
begin
m_broadPhase.Validate;
end;
function Tb2World.GetProxyCount: Int32;
begin
Result := m_broadPhase.m_proxyCount;
end;
function Tb2World.GetPairCount: Int32;
begin
Result := m_broadPhase.m_pairManager.m_pairCount;
end;
procedure Tb2World.SetGravity(const gravity: TVector2);
begin
m_gravity := gravity;
end;
procedure Tb2World.WakeAllSleepingBodies;
var
b: Tb2Body;
begin
b := m_bodyList;
while Assigned(b) do
begin
if b.IsDynamic and b.IsSleeping then
b.WakeUp;
b := b.GetNext;
end;
end;
////////////////////////////////////////////////////
// Contact
{ Tb2ContactFilter }
function Tb2ContactFilter.ShouldCollide(shape1, shape2: Tb2Shape): Boolean;
begin
with shape1.m_filter do
if (groupIndex = shape2.m_filter.groupIndex) and (groupIndex <> 0) then
Result := groupIndex > 0
else
Result := ((maskBits and shape2.m_filter.categoryBits) <> 0) and
((categoryBits and shape2.m_filter.maskBits) <> 0);
end;
{ Tb2ContactListener }
procedure Tb2ContactListener.Add(var point: Tb2ContactPoint);
begin
end;
procedure Tb2ContactListener.Persist(var point: Tb2ContactPoint);
begin
end;
procedure Tb2ContactListener.Remove(var point: Tb2ContactPoint);
begin
end;
procedure Tb2ContactListener.Result(var point: Tb2ContactResult);
begin
end;
{ Tb2Contact }
constructor Tb2Contact.Create;
begin
m_flags := 0;
m_shape1 := nil;
m_shape2 := nil;
end;
constructor Tb2Contact.Create(shape1, shape2: Tb2Shape);
begin
Create;
if shape1.IsSensor or shape2.IsSensor then
m_flags := m_flags or e_nonSolidFlag;
m_shape1 := shape1;
m_shape2 := shape2;
m_manifoldCount := 0;
m_friction := b2MixFriction(m_shape1.GetFriction, m_shape2.GetFriction);
m_restitution := b2MixRestitution(m_shape1.GetRestitution, m_shape2.GetRestitution);
m_prev := nil;
m_next := nil;
m_node1.contact := nil;
m_node1.prev := nil;
m_node1.next := nil;
m_node1.other := nil;
m_node2.contact := nil;
m_node2.prev := nil;
m_node2.next := nil;
m_node2.other := nil;
end;
class function Tb2Contact.CreateContact(Shape1, Shape2: Tb2Shape): Tb2Contact;
var
i: Integer;
p: Pb2Manifold;
begin
with ContactCreateRecords[Shape1.GetType][Shape2.GetType] do
begin
if Primary then
Result := ClassType.Create(Shape1, Shape2)
else
begin
Result := ClassType.Create(Shape2, Shape1);
p := Result.GetManifolds;
for i := 0 to Result.GetManifoldCount - 1 do
{$IFDEF D2009UP}
{$IFDEF OP_OVERLOAD}
(p + i)^.normal.SetNegative;
{$ELSE}
SetNegative((p + i)^.normal);
{$ENDIF}
{$ELSE}
{$IFDEF OP_OVERLOAD}
Pb2Manifold(Integer(p) + i * SizeOf(Tb2Manifold))^.normal.SetNegative;
{$ELSE}
SetNegative(Pb2Manifold(Integer(p) + i * SizeOf(Tb2Manifold))^.normal);
{$ENDIF}
{$ENDIF}
end;
end;
end;
function Tb2Contact.IsSolid: Boolean;
begin
Result := (m_flags and e_nonSolidFlag) = 0;
end;
procedure Tb2Contact.Update(listener: Tb2ContactListener);
var
oldCount, newCount: Int32;
body1, body2: Tb2Body;
begin
oldCount := m_manifoldCount;
Evaluate(listener);
newCount := m_manifoldCount;
body1 := m_shape1.GetBody;
body2 := m_shape2.GetBody;
if (newCount = 0) and (oldCount > 0) then
begin
body1.WakeUp;
body2.WakeUp;
end;
// Slow contacts don't generate TOI events.
if body1.IsStatic or body1.IsBullet or body2.IsStatic or body2.IsBullet then
m_flags := m_flags and (not e_slowFlag)
else
m_flags := m_flags or e_slowFlag;
end;
{ Tb2ContactSolver }
constructor Tb2ContactSolver.Create(const step: Tb2TimeStep;
contacts: TList; contactCount: Int32);
var
i, j, k: Integer;
contact: Tb2Contact;
b1, b2: Tb2Body;
manifolds, manifold: Pb2Manifold;
count: Int32;
friction, restitution: Float;
w1, w2: Float;
v1, v2: TVector2;
normal, tangent: TVector2;
c: Pb2ContactConstraint;
cp: Pb2ManifoldPoint;
ccp: Pb2ContactConstraintPoint;
r1Sqr, r2Sqr, rn1, rn2: Float;
kNormal, kEqualized: Float;
rt1, rt2, kTangent: Float;
vRel: Float;
{$IFNDEF OP_OVERLOAD}tmpV: TVector2;{$ENDIF}
begin
//m_step := step;
m_constraintCount := 0;
for i := 0 to contactCount - 1 do
begin
//b2Assert(contacts[i]->IsSolid());
m_constraintCount := m_constraintCount + Tb2Contact(contacts[i]).GetManifoldCount;
end;
m_constraints := Pb2ContactConstraint(GetMemory(m_constraintCount * SizeOf(Tb2ContactConstraint)));
count := 0;
for i := 0 to contactCount - 1 do
begin
contact := Tb2Contact(contacts[i]);
b1 := contact.m_shape1.GetBody;
b2 := contact.m_shape2.GetBody;
manifolds := contact.GetManifolds;
friction := contact.m_friction;
restitution := contact.m_restitution;
v1 := b1.m_linearVelocity;
v2 := b2.m_linearVelocity;
w1 := b1.m_angularVelocity;
w2 := b2.m_angularVelocity;
for j := 0 to contact.GetManifoldCount - 1 do
begin
{$IFDEF D2009UP}
manifold := manifolds + j;
{$ELSE}
manifold := Pb2Manifold(Integer(manifolds) + j * SizeOf(Tb2Manifold));
{$ENDIF}
//b2Assert(manifold->pointCount > 0);
normal := manifold^.normal;
//b2Assert(count < m_constraintCount);
{$IFDEF D2009UP}
c := m_constraints + count;
{$ELSE}
c := Pb2ContactConstraint(Integer(m_constraints) + count * SizeOf(Tb2ContactConstraint));
{$ENDIF}
c.body1 := b1;
c.body2 := b2;
c.manifold := manifold;
c.normal := normal;
c.pointCount := manifold^.pointCount;
c.friction := friction;
c.restitution := restitution;
for k := 0 to c.pointCount - 1 do
begin
cp := @manifold^.points[k];
ccp := @c^.points[k];
with ccp^ do
begin
normalImpulse := cp^.normalImpulse;
tangentImpulse := cp^.tangentImpulse;
separation := cp^.separation;
positionImpulse := 0.0;
localAnchor1 := cp^.localPoint1;
localAnchor2 := cp^.localPoint2;
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(b1.m_xf.R, cp^.localPoint1 - b1.GetLocalCenter);
r2 := b2Mul(b2.m_xf.R, cp^.localPoint2 - b2.GetLocalCenter);
{$ELSE}
r1 := b2Mul(b1.m_xf.R, Subtract(cp^.localPoint1, b1.GetLocalCenter));
r2 := b2Mul(b2.m_xf.R, Subtract(cp^.localPoint2, b2.GetLocalCenter));
{$ENDIF}
r1Sqr := b2Dot(r1, r1);
r2Sqr := b2Dot(r2, r2);
rn1 := b2Dot(r1, normal);
rn2 := b2Dot(r2, normal);
kNormal := b1.m_invMass + b2.m_invMass + b1.m_invI * (r1Sqr -
rn1 * rn1) + b2.m_invI * (r2Sqr - rn2 * rn2);
//b2Assert(kNormal > B2_FLT_EPSILON);
normalMass := 1.0 / kNormal;
kEqualized := b1.m_mass * b1.m_invMass + b2.m_mass * b2.m_invMass +
b1.m_mass * b1.m_invI * (r1Sqr - rn1 * rn1) + b2.m_mass *
b2.m_invI * (r2Sqr - rn2 * rn2);
//b2Assert(kEqualized > B2_FLT_EPSILON);
equalizedMass := 1.0 / kEqualized;
tangent := b2Cross(normal, 1.0);
rt1 := b2Dot(r1, tangent);
rt2 := b2Dot(r2, tangent);
kTangent := b1.m_invMass + b2.m_invMass + b1.m_invI * (r1Sqr -
rt1 * rt1) + b2.m_invI * (r2Sqr - rt2 * rt2);
//b2Assert(kTangent > B2_FLT_EPSILON);
tangentMass := 1.0 / kTangent;
// Setup a velocity bias for restitution.
velocityBias := 0.0;
if separation > 0.0 then
velocityBias := -60.0 * separation; // TODO_ERIN b2TimeStep
{$IFDEF OP_OVERLOAD}
vRel := b2Dot(c.normal, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, ccp.r1));
{$ELSE}
tmpV := Subtract(v2, v1);
AddBy(tmpV, b2Cross(w2, r2));
SubtractBy(tmpV, b2Cross(w1, ccp.r1));
vRel := b2Dot(c.normal, tmpV);
{$ENDIF}
if vRel < -b2_velocityThreshold then
velocityBias := velocityBias - c.restitution * vRel;
end;
end;
Inc(count);
end;
end;
//b2Assert(count == m_constraintCount);
end;
destructor Tb2ContactSolver.Destroy;
begin
FreeMemory(m_constraints);
end;
procedure Tb2ContactSolver.InitVelocityConstraints(const step: Tb2TimeStep);
var
i, j: Integer;
b1, b2: Tb2Body;
c: Pb2ContactConstraint;
invMass1, invI1, invMass2, invI2: Float;
normal, tangent: TVector2;
P: TVector2;
begin
// Warm start.
for i := 0 to m_constraintCount - 1 do
begin
{$IFDEF D2009UP}
c := m_constraints + i;
{$ELSE}
c := Pb2ContactConstraint(Integer(m_constraints) + i * SizeOf(Tb2ContactConstraint));
{$ENDIF}
b1 := c.body1;
b2 := c.body2;
invMass1 := b1.m_invMass;
invI1 := b1.m_invI;
invMass2 := b2.m_invMass;
invI2 := b2.m_invI;
normal := c.normal;
tangent := b2Cross(normal, 1.0);
if step.warmStarting then
begin
for j := 0 to c.pointCount - 1 do
with c^.points[j] do
begin
normalImpulse := normalImpulse * step.dtRatio;
tangentImpulse := tangentImpulse * step.dtRatio;
{$IFDEF OP_OVERLOAD}
P := normalImpulse * normal + tangentImpulse * tangent;
b1.m_linearVelocity.SubtractBy(invMass1 * P);
b2.m_linearVelocity.AddBy(invMass2 * P);
{$ELSE}
P := Multiply(normal, normalImpulse);
AddBy(P, Multiply(tangent, tangentImpulse));
SubtractBy(b1.m_linearVelocity, Multiply(P, invMass1));
AddBy(b2.m_linearVelocity, Multiply(P, invMass2));
{$ENDIF}
b1.m_angularVelocity := b1.m_angularVelocity - invI1 * b2Cross(r1, P);
b2.m_angularVelocity := b2.m_angularVelocity + invI2 * b2Cross(r2, P);
end;
end
else
for j := 0 to c.pointCount - 1 do
with c^.points[j] do
begin
normalImpulse := 0.0;
tangentImpulse := 0.0;
end;
end;
end;
procedure Tb2ContactSolver.SolveVelocityConstraints;
var
i, j: Integer;
b1, b2: Tb2Body;
w1, w2: Float;
v1, v2: TVector2;
invMass1, invI1, invMass2, invI2: Float;
normal, tangent: TVector2;
friction: Float;
c: Pb2ContactConstraint;
dv, P: TVector2;
vn, vt, lambda, newImpulse, maxFriction: Float;
begin
for i := 0 to m_constraintCount - 1 do
begin
{$IFDEF D2009UP}
c := m_constraints + i;
{$ELSE}
c := Pb2ContactConstraint(Integer(m_constraints) + i * SizeOf(Tb2ContactConstraint));
{$ENDIF}
b1 := c^.body1;
b2 := c^.body2;
w1 := b1.m_angularVelocity;
w2 := b2.m_angularVelocity;
v1 := b1.m_linearVelocity;
v2 := b2.m_linearVelocity;
invMass1 := b1.m_invMass;
invI1 := b1.m_invI;
invMass2 := b2.m_invMass;
invI2 := b2.m_invI;
normal := c^.normal;
tangent := b2Cross(normal, 1.0);
friction := c^.friction;
// Solve normal constraints
for j := 0 to c^.pointCount - 1 do
with c^.points[j] do
begin
// Relative velocity at contact
{$IFDEF OP_OVERLOAD}
dv := v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
{$ELSE}
dv := Subtract(v2, v1);
AddBy(dv, b2Cross(w2, r2));
SubtractBy(dv, b2Cross(w1, r1));
{$ENDIF}
// Compute normal impulse
vn := b2Dot(dv, normal);
lambda := normalMass * (velocityBias - vn);
// b2Clamp the accumulated impulse
newImpulse := b2Max(normalImpulse + lambda, 0.0);
lambda := newImpulse - normalImpulse;
// Apply contact impulse
{$IFDEF OP_OVERLOAD}
P := lambda * normal;
v1 := v1 - invMass1 * P;
v2 := v2 + invMass2 * P;
{$ELSE}
P := Multiply(normal, lambda);
SubtractBy(v1, Multiply(P, invMass1));
AddBy(v2, Multiply(P, invMass2));
{$ENDIF}
w1 := w1 - invI1 * b2Cross(r1, P);
w2 := w2 + invI2 * b2Cross(r2, P);
normalImpulse := newImpulse;
end;
// Solve tangent constraints
for j := 0 to c^.pointCount - 1 do
with c^.points[j] do
begin
// Relative velocity at contact
{$IFDEF OP_OVERLOAD}
dv := v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
{$ELSE}
dv := Subtract(v2, v1);
AddBy(dv, b2Cross(w2, r2));
SubtractBy(dv, b2Cross(w1, r1));
{$ENDIF}
// Compute tangent force
vt := b2Dot(dv, tangent);
lambda := tangentMass * (-vt);
// b2Clamp the accumulated force
maxFriction := friction * normalImpulse;
newImpulse := b2Clamp(tangentImpulse + lambda, -maxFriction, maxFriction);
lambda := newImpulse - tangentImpulse;
// Apply contact impulse
{$IFDEF OP_OVERLOAD}
P := lambda * tangent;
v1 := v1 - invMass1 * P;
v2 := v2 + invMass2 * P;
{$ELSE}
P := Multiply(tangent, lambda);
SubtractBy(v1, Multiply(P, invMass1));
AddBy(v2, Multiply(P, invMass2));
{$ENDIF}
w1 := w1 - invI1 * b2Cross(r1, P);
w2 := w2 + invI2 * b2Cross(r2, P);
tangentImpulse := newImpulse;
end;
b1.m_linearVelocity := v1;
b1.m_angularVelocity := w1;
b2.m_linearVelocity := v2;
b2.m_angularVelocity := w2;
end;
end;
procedure Tb2ContactSolver.FinalizeVelocityConstraints;
var
i, j: Integer;
m: Pb2Manifold;
c: Pb2ContactConstraint;
begin
for i := 0 to m_constraintCount - 1 do
begin
{$IFDEF D2009UP}
c := m_constraints + i;
{$ELSE}
c := Pb2ContactConstraint(Integer(m_constraints) + i * SizeOf(Tb2ContactConstraint));
{$ENDIF}
m := c^.manifold;
for j := 0 to c^.pointCount - 1 do
begin
m^.points[j].normalImpulse := c^.points[j].normalImpulse;
m^.points[j].tangentImpulse := c^.points[j].tangentImpulse;
end;
end;
end;
function Tb2ContactSolver.SolvePositionConstraints(baumgarte: Float): Boolean;
var
i, j: Integer;
b1, b2: Tb2Body;
c: Pb2ContactConstraint;
minSeparation: Float;
invMass1, invI1, invMass2, invI2: Float;
normal: TVector2;
_r1, _r2: TVector2;
p1, p2, dp: TVector2;
_separation: Float;
dImpulse, impulse0: Float;
impulse: TVector2;
begin
minSeparation := 0.0;
for i := 0 to m_constraintCount - 1 do
begin
{$IFDEF D2009UP}
c := m_constraints + i;
{$ELSE}
c := Pb2ContactConstraint(Integer(m_constraints) + i * SizeOf(Tb2ContactConstraint));
{$ENDIF}
b1 := c.body1;
b2 := c.body2;
invMass1 := b1.m_mass * b1.m_invMass;
invI1 := b1.m_mass * b1.m_invI;
invMass2 := b2.m_mass * b2.m_invMass;
invI2 := b2.m_mass * b2.m_invI;
normal := c^.normal;
// Solver normal constraints
for j := 0 to c^.pointCount - 1 do
with c^.points[j] do
begin
{$IFDEF OP_OVERLOAD}
_r1 := b2Mul(b1.m_xf.R, localAnchor1 - b1.GetLocalCenter);
_r2 := b2Mul(b2.m_xf.R, localAnchor2 - b2.GetLocalCenter);
p1 := b1.m_sweep.c + _r1;
p2 := b2.m_sweep.c + _r2;
dp := p2 - p1;
{$ELSE}
_r1 := b2Mul(b1.m_xf.R, Subtract(localAnchor1, b1.GetLocalCenter));
_r2 := b2Mul(b2.m_xf.R, Subtract(localAnchor2, b2.GetLocalCenter));
p1 := Add(b1.m_sweep.c, _r1);
p2 := Add(b2.m_sweep.c, _r2);
dp := Subtract(p2, p1);
{$ENDIF}
// Approximate the current separation.
_separation := b2Dot(dp, normal) + separation;
// Track max constraint error.
minSeparation := b2Min(minSeparation, _separation);
// Compute normal impulse
dImpulse := -equalizedMass * baumgarte * b2Clamp(_separation +
b2_linearSlop, -b2_maxLinearCorrection, 0.0);
// b2Clamp the accumulated impulse
impulse0 := positionImpulse;
positionImpulse := b2Max(impulse0 + dImpulse, 0.0);
dImpulse := positionImpulse - impulse0;
{$IFDEF OP_OVERLOAD}
impulse := dImpulse * normal;
b1.m_sweep.c.SubtractBy(invMass1 * impulse);
{$ELSE}
impulse := Multiply(normal, dImpulse);
SubtractBy(b1.m_sweep.c, Multiply(impulse, invMass1));
{$ENDIF}
b1.m_sweep.a := b1.m_sweep.a - invI1 * b2Cross(_r1, impulse);
b1.SynchronizeTransform;
{$IFDEF OP_OVERLOAD}
b2.m_sweep.c.AddBy(invMass2 * impulse);
{$ELSE}
AddBy(b2.m_sweep.c, Multiply(impulse, invMass2));
{$ENDIF}
b2.m_sweep.a := b2.m_sweep.a + invI2 * b2Cross(_r2, impulse);
b2.SynchronizeTransform;
end;
end;
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
Result := minSeparation >= -1.5 * b2_linearSlop;
end;
{ Tb2NullContact }
procedure Tb2NullContact.Evaluate(listener: Tb2ContactListener);
begin
end;
function Tb2NullContact.GetManifolds: Pb2Manifold;
begin
Result := nil;
end;
{ Tb2ContactManager }
constructor Tb2ContactManager.Create(world: Tb2World);
begin
m_world := world;
m_destroyImmediate := False;
end;
function Tb2ContactManager.PairAdded(proxyUserData1,
proxyUserData2: Pointer): Pointer;
var
shape1, shape2: Tb2Shape;
body1, body2: Tb2Body;
c: Tb2Contact;
begin
shape1 := Tb2Shape(proxyUserData1);
shape2 := Tb2Shape(proxyUserData2);
body1 := shape1.GetBody;
body2 := shape2.GetBody;
if body1.IsStatic() and body2.IsStatic() then
begin
Result := @m_nullContact;
Exit;
end;
if shape1.GetBody = shape2.GetBody then
begin
Result := @m_nullContact;
Exit;
end;
if body2.IsConnected(body1) then
begin
Result := @m_nullContact;
Exit;
end;
if Assigned(m_world.m_contactFilter) and
(not m_world.m_contactFilter.ShouldCollide(shape1, shape2)) then
begin
Result := @m_nullContact;
Exit;
end;
// Call the factory.
c := Tb2Contact.CreateContact(shape1, shape2);
// Contact creation may swap shapes.
shape1 := c.GetShape1;
shape2 := c.GetShape2;
body1 := shape1.GetBody;
body2 := shape2.GetBody;
// Insert into the world.
c.m_prev := nil;
c.m_next := m_world.m_contactList;
if Assigned(m_world.m_contactList) then
m_world.m_contactList.m_prev := c;
m_world.m_contactList := c;
// Connect to island graph.
// Connect to body 1
c.m_node1.contact := c;
c.m_node1.other := body2;
c.m_node1.prev := nil;
c.m_node1.next := body1.m_contactList;
if Assigned(body1.m_contactList) then
body1.m_contactList.prev := @c.m_node1;
body1.m_contactList := @c.m_node1;
// Connect to body 2
c.m_node2.contact := c;
c.m_node2.other := body1;
c.m_node2.prev := nil;
c.m_node2.next := body2.m_contactList;
if Assigned(body2.m_contactList) then
body2.m_contactList.prev := @c.m_node2;
body2.m_contactList := @c.m_node2;
Inc(m_world.m_contactCount);
Result := c;
end;
procedure Tb2ContactManager.PairRemoved(proxyUserData1, proxyUserData2,
pairUserData: Pointer);
var
c: Tb2Contact;
begin
//B2_NOT_USED(proxyUserData1);
//B2_NOT_USED(proxyUserData2);
if not Assigned(pairUserData) then
Exit;
c := Tb2Contact(pairUserData);
if c = @m_nullContact then
Exit;
// An attached body is being destroyed, we must destroy this contact
// immediately to avoid orphaned shape pointers.
Destroy(c);
end;
procedure Tb2ContactManager.Destroy(c: Tb2Contact);
var
manifoldCount: Int32;
manifolds, manifold: Pb2Manifold;
cp: Tb2ContactPoint;
b1, b2: Tb2Body;
i, j: Integer;
v1, v2: TVector2;
begin
b1 := c.GetShape1.GetBody;
b2 := c.GetShape2.GetBody;
// Inform the user that this contact is ending.
manifoldCount := c.GetManifoldCount;
if (manifoldCount > 0) and Assigned(m_world.m_contactListener) then
begin
manifolds := c.GetManifolds;
cp.shape1 := c.GetShape1;
cp.shape2 := c.GetShape2;
cp.friction := c.m_friction;
cp.restitution := c.m_restitution;
for i := 0 to manifoldCount - 1 do
begin
{$IFDEF D2009UP}
manifold := @manifolds[i];
{$ELSE}
manifold := Pb2Manifold(Integer(manifolds) + i * SizeOf(Tb2Manifold));
{$ENDIF}
cp.normal := manifold^.normal;
for j := 0 to manifold^.pointCount - 1 do
with manifold^.points[j] do
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.separation := separation;
cp.id := id;
m_world.m_contactListener.Remove(cp);
end;
end;
end;
// Remove from the world.
if Assigned(c.m_prev) then
c.m_prev.m_next := c.m_next;
if Assigned(c.m_next) then
c.m_next.m_prev := c.m_prev;
if (c = m_world.m_contactList) then
m_world.m_contactList := c.m_next;
// Remove from body 1
if Assigned(c.m_node1.prev) then
c.m_node1.prev.next := c.m_node1.next;
if Assigned(c.m_node1.next) then
c.m_node1.next.prev := c.m_node1.prev;
if @c.m_node1 = b1.m_contactList then
b1.m_contactList := c.m_node1.next;
// Remove from body 2
if Assigned(c.m_node2.prev) then
c.m_node2.prev.next := c.m_node2.next;
if Assigned(c.m_node2.next) then
c.m_node2.next.prev := c.m_node2.prev;
if @c.m_node2 = b2.m_contactList then
b2.m_contactList := c.m_node2.next;
c.Free;
Dec(m_world.m_contactCount);
end;
procedure Tb2ContactManager.Collide;
var
c: Tb2Contact;
begin
// Update awake contacts.
c := m_world.m_contactList;
while Assigned(c) do
begin
if c.GetShape1.GetBody.IsSleeping and
c.GetShape2.GetBody.IsSleeping then
begin
c := c.GetNext;
Continue;
end;
c.Update(m_world.m_contactListener);
c := c.GetNext;
end;
end;
////////////////////////////////////////////////////
// Island
{ Tb2Island }
(*
Position Correction Notes
=========================
I tried the several algorithms for position correction of the 2D revolute joint.
I looked at these systems:
- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
- suspension bridge with 30 1m long planks of length 1m.
- multi-link chain with 30 1m long links.
Here are the algorithms:
Baumgarte - A fraction of the position error is added to the velocity error. There is no
separate position solver.
Pseudo Velocities - After the velocity solver and position integration,
the position error, Jacobian, and effective mass are recomputed. Then
the velocity constraints are solved with pseudo velocities and a fraction
of the position error is added to the pseudo velocity error. The pseudo
velocities are initialized to zero and there is no warm-starting. After
the position solver, the pseudo velocities are added to the positions.
This is also called the First Order World method or the Position LCP method.
Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
position error is re-computed for each constraint and the positions are updated
after the constraint is solved. The radius vectors (aka Jacobians) are
re-computed too (otherwise the algorithm has horrible instability). The pseudo
velocity states are not needed because they are effectively zero at the beginning
of each iteration. Since we have the current position error, we allow the
iterations to terminate early if the error becomes smaller than b2_linearSlop.
Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
each time a constraint is solved.
Here are the results:
Baumgarte - this is the cheapest algorithm but it has some stability problems,
especially with the bridge. The chain links separate easily close to the root
and they jitter as they struggle to pull together. This is one of the most common
methods in the field. The big drawback is that the position correction artificially
affects the momentum, thus leading to instabilities and False bounce. I used a
bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
factor makes joints and contacts more spongy.
Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
stable. However, joints still separate with large angular velocities. Drag the
simple pendulum in a circle quickly and the joint will separate. The chain separates
easily and does not recover. I used a bias factor of 0.2. A larger value lead to
the bridge collapsing when a heavy cube drops on it.
Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
Velocities, but in other ways it is worse. The bridge and chain are much more
stable, but the simple pendulum goes unstable at high angular velocities.
Full NGS - stable in all tests. The joints display good stiffness. The bridge
still sags, but this is better than infinite forces.
Recommendations
Pseudo Velocities are not really worthwhile because the bridge and chain cannot
recover from joint separation. In other cases the benefit over Baumgarte is small.
Modified NGS is not a robust method for the revolute joint due to the violent
instability seen in the simple pendulum. Perhaps it is viable with other constraint
types, especially scalar constraints where the effectivprocedure Tb2Island.Solve(const step: Tb2TimeStep; const gravity: TVector2;
correctPositions, allowSleep: Boolean);
begin
end;
procedure Tb2Island.SolveTOI(const subStep: Tb2TimeStep);
begin
end;
e mass is a scalar.
This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
and is very fast. I don't think we can escape Baumgarte, especially in highly
demanding cases where high constraint fidelity is not needed.
Full NGS is robust and easy on the eyes. I recommend this as an option for
higher fidelity simulation and certainly for suspension bridges and long chains.
Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
joint separation can be problematic. The number of NGS iterations can be reduced
for better performance without harming robustness much.
Each joint in a can be handled differently in the position solver. So I recommend
a system where the user can select the algorithm on a per joint basis. I would
probably default to the slower Full NGS and let the user select the faster
Baumgarte method in performance critical scenarios.
*)
constructor Tb2Island.Create(bodyCapacity, contactCapacity, jointCapacity: Int32;
listener: Tb2ContactListener);
begin
m_bodyCapacity := bodyCapacity;
m_contactCapacity := contactCapacity;
m_jointCapacity := jointCapacity;
m_bodyCount := 0;
m_contactCount := 0;
m_jointCount := 0;
m_listener := listener;
m_bodies := TList.Create;
m_bodies.Count := bodyCapacity;
m_contacts := TList.Create;
m_contacts.Count := contactCapacity;
m_joints := TList.Create;
m_joints.Count := jointCapacity;
m_positionIterationCount := 0;
end;
destructor Tb2Island.Destroy;
begin
// Warning: the order should reverse the constructor order.
m_joints.Free;
m_contacts.Free;
m_bodies.Free;
inherited;
end;
procedure Tb2Island.Clear;
begin
m_bodyCount := 0;
m_contactCount := 0;
m_jointCount := 0;
end;
procedure Tb2Island.Solve(const step: Tb2TimeStep; const gravity: TVector2;
correctPositions, allowSleep: Boolean);
const
linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
var
i, j: Integer;
b: Tb2Body;
contactSolver: Tb2ContactSolver;
contactsOkay, jointsOkay: Boolean;
minSleepTime: Float;
begin
// Integrate velocities and apply damping.
for i := 0 to m_bodyCount - 1 do
begin
b := Tb2Body(m_bodies[i]);
with b do
begin
if IsStatic() then
Continue;
// Integrate velocities.
{$IFDEF OP_OVERLOAD}
m_linearVelocity.AddBy(step.dt * (gravity + m_invMass * m_force));
{$ELSE}
AddBy(m_linearVelocity, Multiply(UPhysics2DTypes.Add(gravity,
Multiply(m_force, m_invMass)), step.dt));
{$ENDIF}
m_angularVelocity := m_angularVelocity + step.dt * m_invI * m_torque;
// Reset forces.
SetZero(m_force);
m_torque := 0.0;
// 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
{$IFDEF OP_OVERLOAD}
m_linearVelocity.MultiplyBy(b2Clamp(1.0 - step.dt * m_linearDamping, 0.0, 1.0));
{$ELSE}
MultiplyBy(m_linearVelocity, b2Clamp(1.0 - step.dt * m_linearDamping, 0.0, 1.0));
{$ENDIF}
m_angularVelocity := m_angularVelocity * b2Clamp(1.0 - step.dt *
b.m_angularDamping, 0.0, 1.0);
// Check for large velocities.
if (b2Dot(m_linearVelocity, m_linearVelocity) > b2_maxLinearVelocitySquared) then
begin
{$IFDEF OP_OVERLOAD}
m_linearVelocity.Normalize;
m_linearVelocity.MultiplyBy(b2_maxLinearVelocity);
{$ELSE}
Normalize(m_linearVelocity);
MultiplyBy(m_linearVelocity, b2_maxLinearVelocity);
{$ENDIF}
end;
if m_angularVelocity * m_angularVelocity > b2_maxAngularVelocitySquared then
if m_angularVelocity < 0.0 then
m_angularVelocity := -b2_maxAngularVelocity
else
m_angularVelocity := b2_maxAngularVelocity;
end;
end;
contactSolver := Tb2ContactSolver.Create(step, m_contacts, m_contactCount);
try
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for i := 0 to m_jointCount - 1 do
Tb2Joint(m_joints[i]).InitVelocityConstraints(step);
// Solve velocity constraints.
if (contactSolver.m_constraintCount > 0) or (m_jointCount > 0) then
for i := 0 to step.maxIterations - 1 do
begin
contactSolver.SolveVelocityConstraints;
for j := 0 to m_jointCount - 1 do
Tb2Joint(m_joints[j]).SolveVelocityConstraints(step);
end;
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints;
// Integrate positions.
for i := 0 to m_bodyCount - 1 do
begin
b := Tb2Body(m_bodies[i]);
with b do
begin
if IsStatic() then
Continue;
// Store positions for continuous collision.
m_sweep.c0 := m_sweep.c;
m_sweep.a0 := m_sweep.a;
// Integrate
{$IFDEF OP_OVERLOAD}
m_sweep.c.AddBy(step.dt * m_linearVelocity);
{$ELSE}
AddBy(m_sweep.c, Multiply(m_linearVelocity, step.dt));
{$ENDIF}
m_sweep.a := m_sweep.a + step.dt * m_angularVelocity;
// Compute new transform
SynchronizeTransform;
// Note: shapes are synchronized later.
end;
end;
if correctPositions then
begin
// Initialize position constraints.
// Contacts don't need initialization.
for i := 0 to m_jointCount - 1 do
Tb2Joint(m_joints[i]).InitPositionConstraints;
// Iterate over constraints.
m_positionIterationCount := 0;
while (m_positionIterationCount < step.maxIterations) do
begin
contactsOkay := contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
jointsOkay := True;
for i := 0 to m_jointCount - 1 do
jointsOkay := Tb2Joint(m_joints[i]).SolvePositionConstraints and jointsOkay;
if contactsOkay and jointsOkay then
Break;
Inc(m_positionIterationCount);
end;
end;
Report(contactSolver.m_constraints);
if allowSleep then
begin
minSleepTime := FLT_MAX;
for i := 0 to m_bodyCount - 1 do
begin
b := Tb2Body(m_bodies[i]);
with b do
begin
if m_invMass = 0.0 then
Continue;
if (m_flags and e_allowSleepFlag) = 0 then
begin
m_sleepTime := 0.0;
minSleepTime := 0.0;
end;
if ((m_flags and e_allowSleepFlag) = 0) or
(m_angularVelocity * m_angularVelocity > angTolSqr) or
(b2Dot(m_linearVelocity, m_linearVelocity) > linTolSqr) then
begin
m_sleepTime := 0.0;
minSleepTime := 0.0;
end
else
begin
m_sleepTime := m_sleepTime + step.dt;
minSleepTime := b2Min(minSleepTime, m_sleepTime);
end;
end;
end;
if minSleepTime >= b2_timeToSleep then
for i := 0 to m_bodyCount - 1 do
begin
b := Tb2Body(m_bodies[i]);
with b do
begin
m_flags := m_flags or e_sleepFlag;
m_linearVelocity := b2Vec2_zero;
m_angularVelocity := 0.0;
end;
end;
end;
finally
contactSolver.Free;
end;
end;
procedure Tb2Island.SolveTOI(const subStep: Tb2TimeStep);
var
b: Tb2Body;
i: Integer;
contactSolver: Tb2ContactSolver;
begin
contactSolver := Tb2ContactSolver.Create(subStep, m_contacts, m_contactCount);
try
// No warm starting needed for TOI events.
// Solve velocity constraints.
for i := 0 to subStep.maxIterations - 1 do
contactSolver.SolveVelocityConstraints;
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for i := 0 to m_bodyCount - 1 do
begin
b := Tb2Body(m_bodies[i]);
with b do
begin
if IsStatic() then
Continue;
// Store positions for continuous collision.
m_sweep.c0 := m_sweep.c;
m_sweep.a0 := m_sweep.a;
// Integrate
{$IFDEF OP_OVERLOAD}
m_sweep.c.AddBy(subStep.dt * m_linearVelocity);
{$ELSE}
AddBy(m_sweep.c, Multiply(m_linearVelocity, subStep.dt));
{$ENDIF}
m_sweep.a := m_sweep.a + subStep.dt * m_angularVelocity;
// Compute new transform
SynchronizeTransform;
// Note: shapes are synchronized later.
end;
end;
// Solve position constraints.
for i := 0 to subStep.maxIterations - 1 do
if contactSolver.SolvePositionConstraints(0.75) then
Break;
Report(contactSolver.m_constraints);
finally
contactSolver.Free;
end;
end;
procedure Tb2Island.Add(body: Tb2Body);
begin
//b2Assert(m_bodyCount < m_bodyCapacity);
m_bodies[m_bodyCount] := body;
Inc(m_bodyCount);
end;
procedure Tb2Island.Add(contact: Tb2Contact);
begin
//b2Assert(m_contactCount < m_contactCapacity);
m_contacts[m_contactCount] := contact;
Inc(m_contactCount);
end;
procedure Tb2Island.Add(joint: Tb2Joint);
begin
//b2Assert(m_jointCount < m_jointCapacity);
m_joints[m_jointCount] := joint;
Inc(m_jointCount);
end;
procedure Tb2Island.Report(constraints: Pb2ContactConstraint);
var
i, j, k: Integer;
c: Tb2Contact;
cc: Pb2ContactConstraint;
cr: Tb2ContactResult;
b1: Tb2Body;
manifoldCount: Int32;
manifolds, manifold: Pb2Manifold;
point: Pb2ManifoldPoint;
begin
if not Assigned(m_listener) then
Exit;
for i := 0 to m_contactCount - 1 do
begin
c := Tb2Contact(m_contacts[i]);
{$IFDEF D2009UP}
cc := constraints + i;
{$ELSE}
cc := Pb2ContactConstraint(Integer(constraints) + i * SizeOf(Tb2ContactConstraint));
{$ENDIF}
cr.shape1 := c.GetShape1;
cr.shape2 := c.GetShape2;
b1 := cr.shape1.GetBody;
manifoldCount := c.GetManifoldCount;
manifolds := c.GetManifolds;
for j := 0 to manifoldCount - 1 do
begin
{$IFDEF D2009UP}
manifold := manifolds + j;
{$ELSE}
manifold := Pb2Manifold(Integer(manifolds) + j * SizeOf(Tb2Manifold));
{$ENDIF}
cr.normal := manifold.normal;
for k := 0 to manifold^.pointCount - 1 do
begin
point := @manifold.points[k];
with cc.points[k] do
begin
cr.position := b1.GetWorldPoint(point^.localPoint1);
// TOI constraint results are not stored, so get
// the result from the constraint.
cr.normalImpulse := normalImpulse;
cr.tangentImpulse := tangentImpulse;
cr.id := point^.id;
m_listener.Result(cr);
end;
end;
end;
end;
end;
////////////////////////////////////////////////////
//
{$IFDEF OP_OVERLOAD}
function Tb2Bound.IsLower: Boolean;
begin
Result := (value and 1) = 0;
end;
function Tb2Bound.IsUpper: Boolean;
begin
Result := (value and 1) = 1;
end;
{$ENDIF}
{$IFDEF OP_OVERLOAD}
function Tb2Proxy.GetNext: UInt16;
begin
Result := lowerBounds[0];
end;
procedure Tb2Proxy.SetNext(Next: UInt16);
begin
lowerBounds[0] := Next;
end;
function Tb2Proxy.IsValid: Boolean;
begin
Result := overlapCount <> b2_invalid;
end;
{$ENDIF}
{ Tb2BroadPhase }
constructor Tb2BroadPhase.Create(const worldAABB: Tb2AABB; callback: Tb2PairCallback);
var
d: TVector2;
i: Integer;
begin
m_pairManager := Tb2PairManager.Create;
m_pairManager.Initialize(Self, callback);
//b2Assert(worldAABB.IsValid());
m_worldAABB := worldAABB;
m_proxyCount := 0;
{$IFDEF OP_OVERLOAD}
d := worldAABB.upperBound - worldAABB.lowerBound;
{$ELSE}
d := Subtract(worldAABB.upperBound, worldAABB.lowerBound);
{$ENDIF}
m_quantizationFactor.x := B2BROADPHASE_MAX / d.x;
m_quantizationFactor.y := B2BROADPHASE_MAX / d.y;
for i := 0 to b2_maxProxies - 2 do
with m_proxyPool[i] do
begin
{$IFDEF OP_OVERLOAD}
SetNext(i + 1);
{$ELSE}
//SetNext(m_proxyPool[i], i + 1);
lowerBounds[0] := i + 1;
{$ENDIF}
timeStamp := 0;
overlapCount := b2_invalid;
userData := nil;
end;
with m_proxyPool[b2_maxProxies - 1] do
begin
{$IFDEF OP_OVERLOAD}
SetNext(b2_nullProxy);
{$ELSE}
//SetNext(m_proxyPool[b2_maxProxies - 1], b2_nullProxy);
lowerBounds[0] := b2_nullProxy;
{$ENDIF}
timeStamp := 0;
overlapCount := b2_invalid;
userData := nil;
end;
m_freeProxy := 0;
m_timeStamp := 1;
m_queryResultCount := 0;
end;
destructor Tb2BroadPhase.Destroy;
begin
m_pairManager.Free;
inherited;
end;
procedure Tb2BroadPhase.ComputeBounds(var lowerValues, upperValues: Tb2BoundValuesArray;
const aabb: Tb2AABB);
var
minVertex, maxVertex: TVector2;
begin
//b2Assert(aabb.upperBound.x > aabb.lowerBound.x);
//b2Assert(aabb.upperBound.y > aabb.lowerBound.y);
minVertex := b2Clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
maxVertex := b2Clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
// Bump lower bounds downs and upper bounds up. This ensures correct sorting of
// lower/upper bounds that would have equal values.
// TODO_ERIN implement fast float to uint16 conversion.
lowerValues[0] := UInt16(Trunc((m_quantizationFactor.x * (minVertex.x -
m_worldAABB.lowerBound.x)))) and (B2BROADPHASE_MAX - 1);
upperValues[0] := UInt16(Trunc((m_quantizationFactor.x * (maxVertex.x -
m_worldAABB.lowerBound.x)))) or 1;
lowerValues[1] := UInt16(Trunc((m_quantizationFactor.y * (minVertex.y -
m_worldAABB.lowerBound.y)))) and (B2BROADPHASE_MAX - 1);
upperValues[1] := UInt16(Trunc((m_quantizationFactor.y * (maxVertex.y -
m_worldAABB.lowerBound.y)))) or 1;
end;
function Tb2BroadPhase.TestOverlap(var p1, p2: Tb2Proxy): Boolean;
var
axis: Integer;
bounds: Pb2AxialBoundsArray;
begin
for axis := 0 to 1 do
begin
bounds := @m_bounds[axis];
//b2Assert(p1->lowerBounds[axis] < 2 * m_proxyCount);
//b2Assert(p1->upperBounds[axis] < 2 * m_proxyCount);
//b2Assert(p2->lowerBounds[axis] < 2 * m_proxyCount);
//b2Assert(p2->upperBounds[axis] < 2 * m_proxyCount);
if bounds^[p1.lowerBounds[axis]].value > bounds^[p2.upperBounds[axis]].value then
begin
Result := False;
Exit;
end;
if bounds^[p1.upperBounds[axis]].value < bounds^[p2.lowerBounds[axis]].value then
begin
Result := False;
Exit;
end;
end;
Result := True;
end;
function Tb2BroadPhase.TestOverlap(const b: Tb2BoundValues; var p: Tb2Proxy): Boolean;
var
axis: Integer;
bounds: Pb2AxialBoundsArray;
begin
for axis := 0 to 1 do
begin
bounds := @m_bounds[axis];
//b2Assert(p->lowerBounds[axis] < 2 * m_proxyCount);
//b2Assert(p->upperBounds[axis] < 2 * m_proxyCount);
if b.lowerValues[axis] > bounds^[p.upperBounds[axis]].value then
begin
Result := False;
Exit;
end;
if b.upperValues[axis] < bounds^[p.lowerBounds[axis]].value then
begin
Result := False;
Exit;
end;
end;
Result := True;
end;
procedure Tb2BroadPhase.Query(var lowerIndex, upperIndex: Int32; lowerValue,
upperValue: UInt16; var bounds: Tb2AxialBoundsArray; boundCount, axis: Int32);
var
i, s: Integer;
lowerQuery, upperQuery: Int32;
begin
lowerQuery := BroadPhase_BinarySearch(bounds, boundCount, lowerValue);
upperQuery := BroadPhase_BinarySearch(bounds, boundCount, upperValue);
// Easy case: lowerQuery <= lowerIndex(i) < upperQuery
// Solution: search query range for min bounds.
for i := lowerQuery to upperQuery - 1 do
begin
{$IFDEF OP_OVERLOAD}
if bounds[i].IsLower() then
{$ELSE}
if IsLower(bounds[i]) then
{$ENDIF}
IncrementOverlapCount(bounds[i].proxyId);
end;
// Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
// Solution: use the stabbing count to search down the bound array.
if lowerQuery > 0 then
begin
i := lowerQuery - 1;
s := bounds[i].stabbingCount;
// Find the s overlaps.
while (s > 0) do //b2Assert(i >= 0);
with bounds[i] do
begin
{$IFDEF OP_OVERLOAD}
if IsLower() then
{$ELSE}
if IsLower(bounds[i]) then
{$ENDIF}
if lowerQuery <= m_proxyPool[proxyId].upperBounds[axis] then
begin
IncrementOverlapCount(proxyId);
Dec(s);
end;
Dec(i);
end;
end;
lowerIndex := lowerQuery;
upperIndex := upperQuery;
end;
procedure Tb2BroadPhase.IncrementOverlapCount(proxyId: Int32);
begin
with m_proxyPool[proxyId] do
if timeStamp < m_timeStamp then
begin
timeStamp := m_timeStamp;
overlapCount := 1;
end
else
begin
overlapCount := 2;
//b2Assert(m_queryResultCount < b2_maxProxies);
m_queryResults[m_queryResultCount] := proxyId;
Inc(m_queryResultCount);
end;
end;
procedure Tb2BroadPhase.IncrementTimeStamp;
var
i: Integer;
begin
if m_timeStamp = B2BROADPHASE_MAX then
begin
for i := 0 to b2_maxProxies - 1 do
m_proxyPool[i].timeStamp := 0;
m_timeStamp := 1;
end
else
Inc(m_timeStamp);
end;
function Tb2BroadPhase.InRange(const aabb: Tb2AABB): Boolean;
var
d: TVector2;
begin
{$IFDEF OP_OVERLOAD}
d := b2Max(aabb.lowerBound - m_worldAABB.upperBound,
m_worldAABB.lowerBound - aabb.upperBound);
{$ELSE}
d := b2Max(Subtract(aabb.lowerBound, m_worldAABB.upperBound),
Subtract(m_worldAABB.lowerBound, aabb.upperBound));
{$ENDIF}
Result := b2Max(d.x, d.y) < 0.0;
end;
function Tb2BroadPhase.CreateProxy(const aabb: Tb2AABB; userData: Pointer): UInt16;
var
AproxyId: UInt16;
proxy: Pb2Proxy;
index, boundCount, axis, lowerIndex, upperIndex: Int32;
lowerValues, upperValues: Tb2BoundValuesArray;
bounds: Pb2AxialBoundsArray;
begin
//b2Assert(m_proxyCount < b2_maxProxies);
//b2Assert(m_freeProxy != b2_nullProxy);
AproxyId := m_freeProxy;
proxy := @(m_proxyPool[AproxyId]);
{$IFDEF OP_OVERLOAD}
m_freeProxy := proxy^.GetNext;
{$ELSE}
m_freeProxy := GetNext(proxy^);
{$ENDIF}
proxy^.overlapCount := 0;
proxy^.userData := userData;
boundCount := 2 * m_proxyCount;
ComputeBounds(lowerValues, upperValues, aabb);
for axis := 0 to 1 do
begin
bounds := @m_bounds[axis];
Query(lowerIndex, upperIndex, lowerValues[axis], upperValues[axis],
bounds^, boundCount, axis);
Move(bounds^[upperIndex], bounds^[upperIndex + 2],
(boundCount - upperIndex) * SizeOf(Tb2Bound));
Move(bounds^[lowerIndex], bounds^[lowerIndex + 1],
(upperIndex - lowerIndex) * SizeOf(Tb2Bound));
// The upper index has increased because of the lower bound insertion.
Inc(upperIndex);
// Copy in the new bounds.
with bounds^[lowerIndex] do
begin
value := lowerValues[axis];
proxyId := AproxyId;
if lowerIndex = 0 then
stabbingCount := 0
else
stabbingCount := bounds^[lowerIndex - 1].stabbingCount;
end;
with bounds^[upperIndex] do
begin
value := upperValues[axis];
proxyId := AproxyId;
stabbingCount := bounds^[upperIndex - 1].stabbingCount;
end;
// Adjust the stabbing count between the new bounds.
for index := lowerIndex to upperIndex - 1 do
Inc(bounds^[index].stabbingCount);
// Adjust the all the affected bound indices.
for index := lowerIndex to boundCount + 1 do
begin
proxy := @m_proxyPool[bounds^[index].proxyId];
{$IFDEF OP_OVERLOAD}
if bounds^[index].IsLower() then
{$ELSE}
if IsLower(bounds^[index]) then
{$ENDIF}
proxy^.lowerBounds[axis] := index
else
proxy^.upperBounds[axis] := index;
end;
end;
Inc(m_proxyCount);
//b2Assert(m_queryResultCount < b2_maxProxies);
// Create pairs if the AABB is in range.
for index := 0 to m_queryResultCount - 1 do
begin
//b2Assert(m_queryResults[i] < b2_maxProxies);
//b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
m_pairManager.AddBufferedPair(AproxyId, m_queryResults[index]);
end;
m_pairManager.Commit;
{$IFDEF CLASSVAR_AVAIL}
if s_validate then
{$ELSE}
if b2BroadPhase_s_validate then
{$ENDIF}
Validate;
// Prepare for next query.
m_queryResultCount := 0;
IncrementTimeStamp;
Result := AproxyId;
end;
procedure Tb2BroadPhase.DestroyProxy(proxyId: Int32);
var
proxy, proxy2: Pb2Proxy;
i, boundCount: Int32;
axis: Integer;
bounds: Pb2AxialBoundsArray;
lowerIndex, upperIndex: Int32;
lowerValue, upperValue: UInt16;
begin
//b2Assert(0 < m_proxyCount && m_proxyCount <= b2_maxProxies);
proxy := @m_proxyPool[proxyId];
//b2Assert(proxy->IsValid());
boundCount := 2 * m_proxyCount;
for axis := 0 to 1 do
begin
bounds := @m_bounds[axis];
lowerIndex := proxy^.lowerBounds[axis];
upperIndex := proxy^.upperBounds[axis];
lowerValue := bounds^[lowerIndex].value;
upperValue := bounds^[upperIndex].value;
Move(bounds^[lowerIndex + 1], bounds^[lowerIndex],
(upperIndex - lowerIndex - 1) * SizeOf(Tb2Bound));
Move(bounds^[upperIndex + 1], bounds^[upperIndex - 1],
(boundCount - upperIndex - 1) * SizeOf(Tb2Bound));
// Fix bound indices.
for i := lowerIndex to boundCount - 3 do
begin
proxy2 := @m_proxyPool[bounds^[i].proxyId];
{$IFDEF OP_OVERLOAD}
if bounds^[i].IsLower() then
{$ELSE}
if IsLower(bounds^[i]) then
{$ENDIF}
proxy2^.lowerBounds[axis] := i
else
proxy2^.upperBounds[axis] := i;
end;
// Fix stabbing count.
for i := lowerIndex to upperIndex - 2 do
Dec(bounds^[i].stabbingCount);
// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
Query(lowerIndex, upperIndex, lowerValue, upperValue,
bounds^, boundCount - 2, axis);
end;
//b2Assert(m_queryResultCount < b2_maxProxies);
for i := 0 to m_queryResultCount - 1 do
begin
//b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
end;
m_pairManager.Commit;
// Prepare for next query.
m_queryResultCount := 0;
IncrementTimeStamp;
// Return the proxy to the pool.
with proxy^ do
begin
userData := nil;
overlapCount := b2_invalid;
lowerBounds[0] := b2_invalid;
lowerBounds[1] := b2_invalid;
upperBounds[0] := b2_invalid;
upperBounds[1] := b2_invalid;
end;
{$IFDEF OP_OVERLOAD}
proxy^.SetNext(m_freeProxy);
{$ELSE}
SetNext(proxy^, m_freeProxy);
{$ENDIF}
m_freeProxy := proxyId;
Dec(m_proxyCount);
{$IFDEF CLASSVAR_AVAIL}
if s_validate then
{$ELSE}
if b2BroadPhase_s_validate then
{$ENDIF}
Validate;
end;
procedure Tb2BroadPhase.MoveProxy(proxyId: Int32; const aabb: Tb2AABB);
var
axis: Integer;
index, boundCount, prevProxyId, nextProxyId: Int32;
lowerIndex, upperIndex, deltaLower, deltaUpper: Int32;
lowerValue, upperValue: UInt16;
proxy, prevProxy, nextProxy: Pb2Proxy;
newValues, oldValues: Tb2BoundValues;
bounds: Pb2AxialBoundsArray;
bound, prevBound, nextBound: Pb2Bound;
begin
if (proxyId = b2_nullProxy) or (b2_maxProxies <= proxyId) then
begin
//b2Assert(False);
Exit;
end;
{$IFDEF OP_OVERLOAD}
if not aabb.IsValid() then
{$ELSE}
if not IsValid(aabb) then
{$ENDIF}
begin
//b2Assert(False);
Exit;;
end;
boundCount := 2 * m_proxyCount;
proxy := @m_proxyPool[proxyId];
// Get new bound values
ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);
// Get old bound values
for axis := 0 to 1 do
begin
oldValues.lowerValues[axis] := m_bounds[axis][proxy^.lowerBounds[axis]].value;
oldValues.upperValues[axis] := m_bounds[axis][proxy^.upperBounds[axis]].value;
end;
for axis := 0 to 1 do
begin
bounds := @m_bounds[axis];
lowerIndex := proxy^.lowerBounds[axis];
upperIndex := proxy^.upperBounds[axis];
lowerValue := newValues.lowerValues[axis];
upperValue := newValues.upperValues[axis];
deltaLower := lowerValue - bounds^[lowerIndex].value;
deltaUpper := upperValue - bounds^[upperIndex].value;
bounds^[lowerIndex].value := lowerValue;
bounds^[upperIndex].value := upperValue;
// Expanding adds overlaps
// Should we move the lower bound down?
if deltaLower < 0 then
begin
index := lowerIndex;
while ((index > 0) and (lowerValue < bounds^[index - 1].value)) do
begin
bound := @bounds^[index];
prevBound := @bounds^[index - 1];
prevProxyId := prevBound^.proxyId;
prevProxy := @m_proxyPool[prevProxyId];
Inc(prevBound^.stabbingCount);
{$IFDEF OP_OVERLOAD}
if prevBound^.IsUpper() then
{$ELSE}
if IsUpper(prevBound^) then
{$ENDIF}
begin
if TestOverlap(newValues, prevProxy^) then
m_pairManager.AddBufferedPair(proxyId, prevProxyId);
Inc(prevProxy^.upperBounds[axis]);
Inc(bound^.stabbingCount);
end
else
begin
Inc(prevProxy^.lowerBounds[axis]);
Dec(bound^.stabbingCount);
end;
Dec(proxy^.lowerBounds[axis]);
b2Swap(bound^, prevBound^);
Dec(index);
end;
end;
// Should we move the upper bound up?
if deltaUpper > 0 then
begin
index := upperIndex;
while ((index < boundCount - 1) and (bounds^[index + 1].value <= upperValue)) do
begin
bound := @bounds^[index];
nextBound := @bounds^[index + 1];
nextProxyId := nextBound^.proxyId;
nextProxy := @m_proxyPool[nextProxyId];
Inc(nextBound^.stabbingCount);
{$IFDEF OP_OVERLOAD}
if nextBound^.IsLower() then
{$ELSE}
if IsLower(nextBound^) then
{$ENDIF}
begin
if TestOverlap(newValues, nextProxy^) then
m_pairManager.AddBufferedPair(proxyId, nextProxyId);
Dec(nextProxy^.lowerBounds[axis]);
Inc(bound^.stabbingCount);
end
else
begin
Dec(nextProxy^.upperBounds[axis]);
Dec(bound^.stabbingCount);
end;
Inc(proxy^.upperBounds[axis]);
b2Swap(bound^, nextBound^);
Inc(index);
end;
end;
// Shrinking removes overlaps
// Should we move the lower bound up?
if deltaLower > 0 then
begin
index := lowerIndex;
while (index < boundCount - 1) and (bounds^[index + 1].value <= lowerValue) do
begin
bound := @bounds^[index];
nextBound := @bounds^[index + 1];
nextProxyId := nextBound^.proxyId;
nextProxy := @m_proxyPool[nextProxyId];
Dec(nextBound^.stabbingCount);
{$IFDEF OP_OVERLOAD}
if nextBound^.IsUpper() then
{$ELSE}
if IsUpper(nextBound^) then
{$ENDIF}
begin
if TestOverlap(oldValues, nextProxy^) then
m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
Dec(nextProxy^.upperBounds[axis]);
Dec(bound^.stabbingCount);
end
else
begin
Dec(nextProxy^.lowerBounds[axis]);
Inc(bound^.stabbingCount);
end;
Inc(proxy^.lowerBounds[axis]);
b2Swap(bound^, nextBound^);
Inc(index);
end;
end;
// Should we move the upper bound down?
if deltaUpper < 0 then
begin
index := upperIndex;
while (index > 0) and (upperValue < bounds^[index - 1].value) do
begin
bound := @bounds^[index];
prevBound := @bounds^[index - 1];
prevProxyId := prevBound^.proxyId;
prevProxy := @m_proxyPool[prevProxyId];
Dec(prevBound^.stabbingCount);
{$IFDEF OP_OVERLOAD}
if prevBound^.IsLower() then
{$ELSE}
if IsLower(prevBound^) then
{$ENDIF}
begin
if TestOverlap(oldValues, prevProxy^) then
m_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
Inc(prevProxy^.lowerBounds[axis]);
Dec(bound^.stabbingCount);
end
else
begin
Inc(prevProxy^.upperBounds[axis]);
Inc(bound^.stabbingCount);
end;
Dec(proxy^.upperBounds[axis]);
b2Swap(bound^, prevBound^);
Dec(index);
end;
end;
end;
{$IFDEF CLASSVAR_AVAIL}
if s_validate then
{$ELSE}
if b2BroadPhase_s_validate then
{$ENDIF}
Validate;
end;
procedure Tb2BroadPhase.Commit;
begin
m_pairManager.Commit;
end;
function Tb2BroadPhase.GetProxy(proxyId: Int32): Pb2Proxy;
begin
{$IFDEF OP_OVERLOAD}
if (proxyId = b2_nullProxy) or (not m_proxyPool[proxyId].IsValid) then
{$ELSE}
if (proxyId = b2_nullProxy) or (not IsValid(m_proxyPool[proxyId])) then
{$ENDIF}
Result := nil
else
Result := @(m_proxyPool[proxyId]);
end;
function Tb2BroadPhase.Query(const aabb: Tb2AABB; userData: TList;
maxCount: Int32): Int32;
var
lowerValues, upperValues: Tb2BoundValuesArray;
lowerIndex, upperIndex: Int32;
count: Int32;
i: Integer;
proxy: Pb2Proxy;
begin
ComputeBounds(lowerValues, upperValues, aabb);
Query(lowerIndex, upperIndex, lowerValues[0], upperValues[0],
m_bounds[0], 2 * m_proxyCount, 0);
Query(lowerIndex, upperIndex, lowerValues[1], upperValues[1],
m_bounds[1], 2 * m_proxyCount, 1);
//b2Assert(m_queryResultCount < b2_maxProxies);
count := 0;
i := 0;
while (i < m_queryResultCount) and (count < maxCount) do
begin
{//b2Assert(m_queryResults[i] < b2_maxProxies);
proxy := @m_proxyPool[m_queryResults[i]];
//b2Assert(proxy->IsValid());
userData[i] := proxy^.userData; }
userData.Add(m_proxyPool[m_queryResults[i]].userData);
Inc(i);
Inc(count);
end;
// Prepare for next query.
m_queryResultCount := 0;
IncrementTimeStamp;
Result := count;
end;
procedure Tb2BroadPhase.Validate;
var
axis: Integer;
bounds: Pb2AxialBoundsArray;
bound: Pb2Bound;
boundCount: Int32;
stabbingCount: UInt16;
i: Integer;
begin
for axis := 0 to 1 do
begin
bounds := @m_bounds[axis];
boundCount := 2 * m_proxyCount;
stabbingCount := 0;
for i := 0 to boundCount - 1 do
begin
bound := @bounds^[i];
//b2Assert(i == 0 || bounds[i-1].value <= bound->value);
//b2Assert(bound->proxyId != b2_nullProxy);
//b2Assert(m_proxyPool[bound->proxyId].IsValid());
{$IFDEF OP_OVERLOAD}
if bound^.IsLower() then
{$ELSE}
if IsLower(bound^) then
{$ENDIF}
begin
//b2Assert(m_proxyPool[bound^.proxyId].lowerBounds[axis] == i);
Inc(stabbingCount);
end
else
begin
//b2Assert(m_proxyPool[bound^.proxyId].upperBounds[axis] == i);
Dec(stabbingCount);
end;
//b2Assert(bound->stabbingCount == stabbingCount);
end;
end;
end;
procedure Tb2BroadPhase.ValidatePairs;
begin
end;
////////////////////////////////////////////////////
// Pair
{$IFDEF OP_OVERLOAD}
procedure Tb2Pair.SetBuffered;
begin
status := status or e_pairBuffered;
end;
procedure Tb2Pair.ClearBuffered;
begin
status := status and (not e_pairBuffered);
end;
function Tb2Pair.IsBuffered: Boolean;
begin
Result := (status and e_pairBuffered) = e_pairBuffered;
end;
procedure Tb2Pair.SetRemoved;
begin
status := status or e_pairRemoved;
end;
procedure Tb2Pair.ClearRemoved;
begin
status := status and (not e_pairRemoved);
end;
function Tb2Pair.IsRemoved: Boolean;
begin
Result := (status and e_pairRemoved) = e_pairRemoved;
end;
procedure Tb2Pair.SetFinal;
begin
status := status or e_pairFinal;
end;
function Tb2Pair.IsFinal: Boolean;
begin
Result := (status and e_pairFinal) = e_pairFinal;
end;
{$ENDIF}
{ Tb2PairManager }
constructor Tb2PairManager.Create;
var
i: Integer;
begin
for i := 0 to b2_tableCapacity - 1 do
m_hashTable[i] := b2_nullPair;
m_freePair := 0;
for i := 0 to b2_maxPairs - 1 do
begin
m_pairs[i].proxyId1 := b2_nullProxy;
m_pairs[i].proxyId2 := b2_nullProxy;
m_pairs[i].userData := nil;
m_pairs[i].status := 0;
m_pairs[i].next := i + 1;
end;
m_pairs[b2_maxPairs - 1].next := b2_nullPair;
m_pairCount := 0;
m_pairBufferCount := 0;
end;
function Tb2PairManager.Find(proxyId1, proxyId2: Int32): Pb2Pair;
var
hash: Int32;
begin
if proxyId1 > proxyId2 then
b2Swap(proxyId1, proxyId2);
hash := PairManager_Hash(proxyId1, proxyId2) and b2_tableMask;
Result := Find(proxyId1, proxyId2, hash);
end;
function Tb2PairManager.Find(proxyId1, proxyId2: Int32; hashValue: UInt32): Pb2Pair;
var
index: Int32;
begin
index := m_hashTable[hashValue];
while (index <> b2_nullPair) and (not PairManager_Equals(m_pairs[index], proxyId1, proxyId2)) do
index := m_pairs[index].next;
if index = b2_nullPair then
Result := nil
else
begin
//b2Assert(index < b2_maxPairs);
Result := Pb2Pair(@m_pairs[index]);
end;
end;
function Tb2PairManager.AddPair(proxyId1, proxyId2: Int32): Pb2Pair;
var
hash: Int32;
pairIndex: Int16;
begin
if proxyId1 > proxyId2 then
b2Swap(proxyId1, proxyId2);
hash := PairManager_Hash(proxyId1, proxyId2) and b2_tableMask;
Result := Find(proxyId1, proxyId2, hash);
if Assigned(Result) then
Exit;
//b2Assert(m_pairCount < b2_maxPairs && m_freePair != b2_nullPair);
pairIndex := m_freePair;
Result := @m_pairs[pairIndex];
m_freePair := Result^.next;
Result^.proxyId1 := proxyId1;
Result^.proxyId2 := proxyId2;
Result^.status := 0;
Result^.userData := nil;
Result^.next := m_hashTable[hash];
m_hashTable[hash] := pairIndex;
Inc(m_pairCount);
end;
function Tb2PairManager.RemovePair(proxyId1, proxyId2: Int32): Pointer;
var
hash: Int32;
index: UInt16;
pair: Pb2Pair;
node: ^UInt16;
userData: Pointer;
begin
//b2Assert(m_pairCount > 0);
if proxyId1 > proxyId2 then
b2Swap(proxyId1, proxyId2);
hash := PairManager_Hash(proxyId1, proxyId2) and b2_tableMask;
node := @m_hashTable[hash];
while (node^ <> b2_nullPair) do
begin
if PairManager_Equals(m_pairs[node^], proxyId1, proxyId2) then
begin
index := node^;
node^ := m_pairs[node^].next;
pair := @(m_pairs[index]);
userData := pair^.userData;
// Scrub
pair^.next := m_freePair;
pair^.proxyId1 := b2_nullProxy;
pair^.proxyId2 := b2_nullProxy;
pair^.userData := nil;
pair^.status := 0;
m_freePair := index;
Dec(m_pairCount);
Result := userData;
Exit;
end
else
node := @m_pairs[node^].next;
end;
//b2Assert(False);
Result := nil;
end;
procedure Tb2PairManager.ValidateBuffer;
begin
(*
#ifdef _DEBUG
b2Assert(m_pairBufferCount <= m_pairCount);
std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount);
for (int32 i = 0; i < m_pairBufferCount; ++i)
{
if (i > 0)
{
b2Assert(Equals(m_pairBuffer[i], m_pairBuffer[i-1]) == False);
}
b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
b2Assert(pair^.IsBuffered());
b2Assert(pair^.proxyId1 != pair^.proxyId2);
b2Assert(pair^.proxyId1 < b2_maxProxies);
b2Assert(pair^.proxyId2 < b2_maxProxies);
b2Proxy* proxy1 = m_broadPhase^.m_proxyPool + pair^.proxyId1;
b2Proxy* proxy2 = m_broadPhase^.m_proxyPool + pair^.proxyId2;
b2Assert(proxy1^.IsValid() == True);
b2Assert(proxy2^.IsValid() == True);
}
#endif
*)
end;
procedure Tb2PairManager.ValidateTable;
begin
(*
#ifdef _DEBUG
for (int32 i = 0; i < b2_tableCapacity; ++i)
{
uint16 index = m_hashTable[i];
while (index != b2_nullPair)
{
b2Pair* pair = m_pairs + index;
b2Assert(pair^.IsBuffered() == False);
b2Assert(pair^.IsFinal() == True);
b2Assert(pair^.IsRemoved() == False);
b2Assert(pair^.proxyId1 != pair^.proxyId2);
b2Assert(pair^.proxyId1 < b2_maxProxies);
b2Assert(pair^.proxyId2 < b2_maxProxies);
b2Proxy* proxy1 = m_broadPhase^.m_proxyPool + pair^.proxyId1;
b2Proxy* proxy2 = m_broadPhase^.m_proxyPool + pair^.proxyId2;
b2Assert(proxy1^.IsValid() == True);
b2Assert(proxy2^.IsValid() == True);
b2Assert(m_broadPhase^.TestOverlap(proxy1, proxy2) == True);
index = pair^.next;
}
}
#endif
*)
end;
procedure Tb2PairManager.Initialize(broadPhase: Tb2BroadPhase;
callback: Tb2PairCallback);
begin
m_broadPhase := broadPhase;
m_callback := callback;
end;
{
As proxies are created and moved, many pairs are created and destroyed. Even worse, the same
pair may be added and removed multiple times in a single time step of the physics engine. To reduce
traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the
end of the physics step. This is done by buffering all the RemovePair requests. AddPair
requests are processed immediately because we need the hash table entry for quick lookup.
All user user callbacks are delayed until the buffered pairs are confirmed in Commit.
This is very important because the user callbacks may be very expensive and client logic
may be harmed if pairs are added and removed within the same time step.
Buffer a pair for addition.
We may add a pair that is not in the pair manager or pair buffer.
We may add a pair that is already in the pair manager and pair buffer.
If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called).
}
procedure Tb2PairManager.AddBufferedPair(proxyId1, proxyId2: Int32);
var
pair: Pb2Pair;
begin
//b2Assert(id1 != b2_nullProxy && id2 != b2_nullProxy);
//b2Assert(m_pairBufferCount < b2_maxPairs);
pair := AddPair(proxyId1, proxyId2);
// If this pair is not in the pair buffer ...
{$IFDEF OP_OVERLOAD}
if not pair^.IsBuffered() then
{$ELSE}
if not IsBuffered(pair^) then
{$ENDIF}
begin
// This must be a newly added pair.
//b2Assert(pair^.IsFinal() == False);
// Add it to the pair buffer.
{$IFDEF OP_OVERLOAD}
pair^.SetBuffered;
{$ELSE}
SetBuffered(pair^);
{$ENDIF}
m_pairBuffer[m_pairBufferCount].proxyId1 := pair^.proxyId1;
m_pairBuffer[m_pairBufferCount].proxyId2 := pair^.proxyId2;
Inc(m_pairBufferCount);
//b2Assert(m_pairBufferCount <= m_pairCount);
end;
// Confirm this pair for the subsequent call to Commit.
{$IFDEF OP_OVERLOAD}
pair^.ClearRemoved;
{$ELSE}
ClearRemoved(pair^);
{$ENDIF}
{$IFDEF CLASSVAR_AVAIL}
if Tb2BroadPhase.s_validate then
{$ELSE}
if b2BroadPhase_s_validate then
{$ENDIF}
ValidateBuffer;
end;
procedure Tb2PairManager.RemoveBufferedPair(proxyId1, proxyId2: Int32);
var
pair: Pb2Pair;
begin
//b2Assert(id1 != b2_nullProxy && id2 != b2_nullProxy);
//b2Assert(m_pairBufferCount < b2_maxPairs);
pair := Find(proxyId1, proxyId2);
if not Assigned(pair) then
// The pair never existed. This is legal (due to collision filtering).
Exit;
// If this pair is not in the pair buffer ...
{$IFDEF OP_OVERLOAD}
if (not pair^.IsBuffered()) then
{$ELSE}
if (not IsBuffered(pair^)) then
{$ENDIF}
begin
// This must be an old pair.
//b2Assert(pair^.IsFinal() == True);
{$IFDEF OP_OVERLOAD}
pair^.SetBuffered;
{$ELSE}
SetBuffered(pair^);
{$ENDIF}
m_pairBuffer[m_pairBufferCount].proxyId1 := pair^.proxyId1;
m_pairBuffer[m_pairBufferCount].proxyId2 := pair^.proxyId2;
Inc(m_pairBufferCount)
//Assert(m_pairBufferCount <= m_pairCount);
end;
{$IFDEF OP_OVERLOAD}
pair^.SetRemoved;
{$ELSE}
SetRemoved(pair^);
{$ENDIF}
{$IFDEF CLASSVAR_AVAIL}
if Tb2BroadPhase.s_validate then
{$ELSE}
if b2BroadPhase_s_validate then
{$ENDIF}
ValidateBuffer;
end;
procedure Tb2PairManager.Commit;
var
i: Integer;
removeCount: Int32;
proxy1, proxy2: Pb2Proxy;
pair: Pb2Pair;
begin
removeCount := 0;
for i := 0 to m_pairBufferCount - 1 do
begin
pair := Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
//b2Assert(pair^.IsBuffered());
{$IFDEF OP_OVERLOAD}
pair^.ClearBuffered;
{$ELSE}
ClearBuffered(pair^);
{$ENDIF}
//b2Assert(pair^.proxyId1 < b2_maxProxies && pair^.proxyId2 < b2_maxProxies);
proxy1 := @m_broadPhase.m_proxyPool[pair^.proxyId1];
proxy2 := @m_broadPhase.m_proxyPool[pair^.proxyId2];
//b2Assert(proxy1^.IsValid());
//b2Assert(proxy2^.IsValid());
{$IFDEF OP_OVERLOAD}
if pair^.IsRemoved() then
{$ELSE}
if IsRemoved(pair^) then
{$ENDIF}
begin
// It is possible a pair was added then removed before a commit. Therefore,
// we should be careful not to tell the user the pair was removed when the
// the user didn't receive a matching add.
{$IFDEF OP_OVERLOAD}
if pair^.IsFinal() then
{$ELSE}
if IsFinal(pair^) then
{$ENDIF}
m_callback.PairRemoved(proxy1^.userData, proxy2^.userData, pair^.userData);
// Store the ids so we can actually remove the pair below.
m_pairBuffer[removeCount].proxyId1 := pair^.proxyId1;
m_pairBuffer[removeCount].proxyId2 := pair^.proxyId2;
Inc(removeCount);
end
else
begin
//b2Assert(m_broadPhase^.TestOverlap(proxy1, proxy2) == True);
{$IFDEF OP_OVERLOAD}
if (not pair^.IsFinal()) then
{$ELSE}
if (not IsFinal(pair^)) then
{$ENDIF}
begin
pair^.userData := m_callback.PairAdded(proxy1^.userData, proxy2^.userData);
{$IFDEF OP_OVERLOAD}
pair^.SetFinal;
{$ELSE}
SetFinal(pair^);
{$ENDIF}
end;
end;
end;
for i := 0 to removeCount - 1 do
RemovePair(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
m_pairBufferCount := 0;
{$IFDEF CLASSVAR_AVAIL}
if Tb2BroadPhase.s_validate then
{$ELSE}
if b2BroadPhase_s_validate then
{$ENDIF}
ValidateBuffer;
end;
{ Tb2ShapeDef }
constructor Tb2ShapeDef.Create;
begin
ShapeType := e_unknownShape;
userData := nil;
friction := 0.2;
restitution := 0.0;
density := 0.0;
filter.categoryBits := $0001;
filter.maskBits := $FFFF;
filter.groupIndex := 0;
isSensor := False;
end;
{ Tb2Shape }
constructor Tb2Shape.Create(def: Tb2ShapeDef);
begin
m_userData := def.userData;
m_friction := def.friction;
m_restitution := def.restitution;
m_density := def.density;
m_body := nil;
m_sweepRadius := 0.0;
m_next := nil;
m_proxyId := b2_nullProxy;
m_filter := def.filter;
m_isSensor := def.isSensor;
end;
destructor Tb2Shape.Destroy;
begin
if Assigned(m_body) then
m_body.DestroyShape(Self, False);
end;
destructor Tb2Shape.Destroy2;
begin
end;
procedure Tb2Shape.CreateProxy(broadPhase: Tb2BroadPhase; const xf: Tb2XForm);
var
aabb: Tb2AABB;
inRange: Boolean;
begin
//b2Assert(m_proxyId == b2_nullProxy);
ComputeAABB(aabb, xf);
inRange := broadPhase.InRange(aabb);
// You are creating a shape outside the world box.
//b2Assert(inRange);
if inRange then
m_proxyId := broadPhase.CreateProxy(aabb, Self)
else
m_proxyId := b2_nullProxy;
end;
procedure Tb2Shape.DestroyProxy(broadPhase: Tb2BroadPhase);
begin
if m_proxyId <> b2_nullProxy then
begin
broadPhase.DestroyProxy(m_proxyId);
m_proxyId := b2_nullProxy;
end;
end;
function Tb2Shape.Synchronize(broadPhase: Tb2BroadPhase; const xf1, xf2: Tb2XForm): Boolean;
var
aabb: Tb2AABB;
begin
if m_proxyId = b2_nullProxy then
begin
Result := False;
Exit;
end;
// Compute an AABB that covers the swept shape (may miss some rotation effect).
ComputeSweptAABB(aabb, xf1, xf2);
if broadPhase.InRange(aabb) then
begin
broadPhase.MoveProxy(m_proxyId, aabb);
Result := True;
end
else
Result := False;
end;
procedure Tb2Shape.RefilterProxy(broadPhase: Tb2BroadPhase; const xf: Tb2XForm);
var
aabb: Tb2AABB;
begin
if m_proxyId = b2_nullProxy then
Exit;
broadPhase.DestroyProxy(m_proxyId);
ComputeAABB(aabb, xf);
if broadPhase.InRange(aabb) then
m_proxyId := broadPhase.CreateProxy(aabb, Self)
else
m_proxyId := b2_nullProxy;
end;
//////////////////////////////////////////////////////////////
// Joints
{ Tb2Jacobian }
{$IFDEF OP_OVERLOAD}
procedure Tb2Jacobian.SetZero;
begin
linear1.SetZero;
linear2.SetZero;
angular1 := 0.0;
angular2 := 0.0;
end;
procedure Tb2Jacobian.SetValue(const x1, x2: TVector2; a1, a2: Float);
begin
linear1 := x1;
linear2 := x2;
angular1 := a1;
angular2 := a2;
end;
function Tb2Jacobian.Compute(const x1, x2: TVector2; a1, a2: Float): Float;
begin
Result := b2Dot(linear1, x1) + angular1 * a1 + b2Dot(linear2, x2) + angular2 * a2;
end;
{$ELSE}
// unfinished
{$ENDIF}
{ Tb2JointDef }
constructor Tb2JointDef.Create;
begin
JointType := e_unknownJoint;
userData := nil;
body1 := nil;
body2 := nil;
collideConnected := False;
end;
{ Tb2Joint }
constructor Tb2Joint.Create(def: Tb2JointDef);
begin
m_type := def.JointType;
m_prev := nil;
m_next := nil;
m_body1 := def.body1;
m_body2 := def.body2;
m_collideConnected := def.collideConnected;
m_islandFlag := False;
m_userData := def.userData;
end;
procedure Tb2Joint.InitPositionConstraints;
begin
end;
//////////////////////////////////////////////////////////////
// Body
{ Tb2BodyDef }
constructor Tb2BodyDef.Create;
begin
SetZero(massData.center);
massData.mass := 0.0;
massData.I := 0.0;
userData := nil;
SetZero(position);
angle := 0.0;
linearDamping := 0.0;
angularDamping := 0.0;
allowSleep := True;
isSleeping := False;
fixedRotation := False;
isBullet := False;
end;
{ Tb2Body }
constructor Tb2Body.Create(bd: Tb2BodyDef; world: Tb2World);
begin
//b2Assert(world->m_lock == False);
m_flags := 0;
if bd.isBullet then
m_flags := m_flags or e_bulletFlag;
if bd.fixedRotation then
m_flags := m_flags or e_fixedRotationFlag;
if bd.allowSleep then
m_flags := m_flags or e_allowSleepFlag;
if bd.isSleeping then
m_flags := m_flags or e_sleepFlag;
m_world := world;
m_xf.position := bd.position;
{$IFDEF OP_OVERLOAD}
m_xf.R.SetValue(bd.angle);
{$ELSE}
SetValue(m_xf.R, bd.angle);
{$ENDIF}
m_sweep.localCenter := bd.massData.center;
m_sweep.t0 := 1.0;
m_sweep.a := bd.angle;
m_sweep.a0 := bd.angle;
m_sweep.c0 := b2Mul(m_xf, m_sweep.localCenter);
m_sweep.c := m_sweep.c0;
m_jointList := nil;
m_contactList := nil;
m_prev := nil;
m_next := nil;
m_linearDamping := bd.linearDamping;
m_angularDamping := bd.angularDamping;
SetZero(m_force);
m_torque := 0.0;
SetZero(m_linearVelocity);
m_angularVelocity := 0.0;
m_sleepTime := 0.0;
m_invMass := 0.0;
m_I := 0.0;
m_invI := 0.0;
m_mass := bd.massData.mass;
if m_mass > 0.0 then
m_invMass := 1.0 / m_mass;
if (m_flags and e_fixedRotationFlag) = 0 then
m_I := bd.massData.I;
if m_I > 0.0 then
m_invI := 1.0 / m_I;
if (m_invMass = 0.0) and (m_invI = 0.0) then
m_type := e_staticType
else
m_type := e_dynamicType;
m_userData := bd.userData;
m_shapeList := nil;
m_shapeCount := 0;
end;
destructor Tb2Body.Destroy;
begin
if Assigned(m_world) then
m_world.DestroyBody(Self, False);
end;
destructor Tb2Body.Destroy2;
begin
end;
function Tb2Body.SynchronizeShapes: Boolean;
var
s: Tb2Shape;
xf1: Tb2XForm;
inRange: Boolean;
begin
{$IFDEF OP_OVERLOAD}
xf1.R.SetValue(m_sweep.a0);
xf1.position := m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
{$ELSE}
SetValue(xf1.R, m_sweep.a0);
xf1.position := Subtract(m_sweep.c0, b2Mul(xf1.R, m_sweep.localCenter));
{$ENDIF}
inRange := True;
s := m_shapeList;
while Assigned(s) do
begin
inRange := s.Synchronize(m_world.m_broadPhase, xf1, m_xf);
if not inRange then
Break;
s := s.m_next;
end;
if not inRange then
begin
m_flags := m_flags or e_frozenFlag;
SetZero(m_linearVelocity);
m_angularVelocity := 0.0;
s := m_shapeList;
while Assigned(s) do
begin
s.DestroyProxy(m_world.m_broadPhase);
s := s.m_next;
end;
// Failure
Result := False;
Exit;
end;
// Success
Result := True;
end;
procedure Tb2Body.SynchronizeTransform;
begin
{$IFDEF OP_OVERLOAD}
m_xf.R.SetValue(m_sweep.a);
m_xf.position := m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter);
{$ELSE}
SetValue(m_xf.R, m_sweep.a);
m_xf.position := Subtract(m_sweep.c, b2Mul(m_xf.R, m_sweep.localCenter));
{$ENDIF}
end;
function Tb2Body.IsConnected(other: Tb2Body): Boolean;
var
jn: Pb2JointEdge;
begin
jn := m_jointList;
while Assigned(jn) do
begin
if jn^.other = other then
begin
Result := jn^.joint.m_collideConnected = False;
Exit;
end;
jn := jn^.next;
end;
Result := False;
end;
procedure Tb2Body.Advance(t: Float);
begin
// Advance to the new safe time.
{$IFDEF OP_OVERLOAD}
m_sweep.Advance(t);
{$ELSE}
UPhysics2DTypes.Advance(m_sweep, t);
{$ENDIF}
m_sweep.c := m_sweep.c0;
m_sweep.a := m_sweep.a0;
SynchronizeTransform;
end;
function Tb2Body.CreateShape(shapeDef: Tb2ShapeDef; AutoFreeShapeDef: Boolean = True): Tb2Shape;
begin
//b2Assert(m_world->m_lock == False);
if m_world.m_lock then
begin
Result := nil;
Exit;
end;
case shapeDef.ShapeType of
e_circleShape: Result := Tb2CircleShape.Create(shapeDef);
e_polygonShape: Result := Tb2PolygonShape.Create(shapeDef);
end;
Result.m_next := m_shapeList;
m_shapeList := Result;
Inc(m_shapeCount);
Result.m_body := Self;
// Add the shape to the world's broad-phase.
Result.CreateProxy(m_world.m_broadPhase, m_xf);
// Compute the sweep radius for CCD.
Result.UpdateSweepRadius(m_sweep.localCenter);
if AutoFreeShapeDef then
shapeDef.Free;
end;
procedure Tb2Body.DestroyShape(s: Tb2Shape; DoFree: Boolean = True);
var
node: Pb2Shape;
found: Boolean;
begin
//b2Assert(m_world->m_lock == False);
if m_world.m_lock then
Exit;
//b2Assert(s->GetBody() == this);
s.DestroyProxy(m_world.m_broadPhase);
//b2Assert(m_shapeCount > 0);
node := @m_shapeList;
found := False;
while (node^ <> nil) do
begin
if node^ = s then
begin
node^ := s.m_next;
found := True;
Break;
end;
node := @node^.m_next;
end;
// You tried to remove a shape that is not attached to this body.
//b2Assert(found);
Dec(m_shapeCount);
if DoFree then
s.Destroy2; // Call a destructor without side effects.
end;
procedure Tb2Body.SetMass(const massData: Tb2MassData);
var
s: Tb2Shape;
oldType: Tb2BodyType;
begin
//b2Assert(m_world->m_lock == False);
if m_world.m_lock then
Exit;
m_invMass := 0.0;
m_I := 0.0;
m_invI := 0.0;
m_mass := massData.mass;
if m_mass > 0.0 then
m_invMass := 1.0 / m_mass;
if (m_flags and e_fixedRotationFlag) = 0 then
m_I := massData.I;
if m_I > 0.0 then
m_invI := 1.0 / m_I;
// Move center of mass.
m_sweep.localCenter := massData.center;
m_sweep.c0 := b2Mul(m_xf, m_sweep.localCenter);
m_sweep.c := m_sweep.c0;
// Update the sweep radii of all child shapes.
s := m_shapeList;
while Assigned(s) do
begin
s.UpdateSweepRadius(m_sweep.localCenter);
s := s.m_next;
end;
oldType := m_type;
if (m_invMass = 0.0) and (m_invI = 0.0) then
m_type := e_staticType
else
m_type := e_dynamicType;
// If the body type changed, we need to refilter the broad-phase proxies.
if oldType <> m_type then
begin
s := m_shapeList;
while Assigned(s) do
begin
s.RefilterProxy(m_world.m_broadPhase, m_xf);
s := s.m_next;
end;
end;
end;
procedure Tb2Body.SetMassFromShapes;
var
s: Tb2Shape;
center: TVector2;
massData: Tb2MassData;
oldType: Tb2BodyType;
begin
//b2Assert(m_world->m_lock == False);
if m_world.m_lock then
Exit;
// Compute mass data from shapes. Each shape has its own density.
m_mass := 0.0;
m_invMass := 0.0;
m_I := 0.0;
m_invI := 0.0;
center := b2Vec2_zero;
s := m_shapeList;
while Assigned(s) do
begin
s.ComputeMass(massData);
m_mass := m_mass + massData.mass;
{$IFDEF OP_OVERLOAD}
center.AddBy(massData.mass * massData.center);
{$ELSE}
AddBy(center, Multiply(massData.center, massData.mass));
{$ENDIF}
m_I := m_I + massData.I;
s := s.m_next;
end;
// Compute center of mass, and shift the origin to the COM.
if m_mass > 0.0 then
begin
m_invMass := 1.0 / m_mass;
{$IFDEF OP_OVERLOAD}
center.MultiplyBy(m_invMass);
{$ELSE}
MultiplyBy(center, m_invMass);
{$ENDIF}
end;
if (m_I > 0.0) and ((m_flags and e_fixedRotationFlag) = 0) then
begin
// Center the inertia about the center of mass.
m_I := m_I - m_mass * b2Dot(center, center);
//b2Assert(m_I > 0.0f);
m_invI := 1.0 / m_I;
end
else
begin
m_I := 0.0;
m_invI := 0.0;
end;
// Move center of mass.
m_sweep.localCenter := center;
m_sweep.c := b2Mul(m_xf, m_sweep.localCenter);
m_sweep.c0 := m_sweep.c;
// Update the sweep radii of all child shapes.
s := m_shapeList;
while Assigned(s) do
begin
s.UpdateSweepRadius(m_sweep.localCenter);
s := s.m_next;
end;
oldType := m_type;
if (m_invMass = 0.0) and (m_invI = 0.0) then
m_type := e_staticType
else
m_type := e_dynamicType;
// If the body type changed, we need to refilter the broad-phase proxies.
if oldType <> m_type then
begin
s := m_shapeList;
while Assigned(s) do
begin
s.RefilterProxy(m_world.m_broadPhase, m_xf);
s := s.m_next;
end;
end;
end;
function Tb2Body.SetXForm(const position: TVector2; angle: Float): Boolean;
var
s: Tb2Shape;
freeze: Boolean;
begin
//b2Assert(m_world->m_lock == False);
if m_world.m_lock then
begin
Result := True;
Exit;
end;
if IsFrozen() then
begin
Result := False;
Exit;
end;
{$IFDEF OP_OVERLOAD}
m_xf.R.SetValue(angle);
{$ELSE}
SetValue(m_xf.R, angle);
{$ENDIF}
m_xf.position := position;
m_sweep.c0 := b2Mul(m_xf, m_sweep.localCenter);
m_sweep.c := m_sweep.c0;
m_sweep.a0 := angle;
m_sweep.a := angle;
freeze := False;
s := m_shapeList;
while Assigned(s) do
begin
if not s.Synchronize(m_world.m_broadPhase, m_xf, m_xf) then
begin
freeze := True;
Break;
end;
s := s.m_next;
end;
if freeze then
begin
m_flags := m_flags or e_frozenFlag;
SetZero(m_linearVelocity);
m_angularVelocity := 0.0;
s := m_shapeList;
while Assigned(s) do
begin
s.DestroyProxy(m_world.m_broadPhase);
s := s.m_next;
end;
// Failure
Result := False;
Exit;
end;
// Success
m_world.m_broadPhase.Commit;
Result := True;
end;
procedure Tb2Body.SetLinearVelocity(const v: TVector2);
begin
m_linearVelocity := v;
end;
procedure Tb2Body.SetAngularVelocity(omega: Float);
begin
m_angularVelocity := omega;
end;
procedure Tb2Body.ApplyForce(const force, point: TVector2);
begin
if IsSleeping() then
WakeUp;
{$IFDEF OP_OVERLOAD}
m_force.AddBy(force);
m_torque := m_torque + b2Cross(point - m_sweep.c, force);
{$ELSE}
AddBy(m_force, force);
m_torque := m_torque + b2Cross(Subtract(point, m_sweep.c), force);
{$ENDIF}
end;
procedure Tb2Body.ApplyTorque(torque: Float);
begin
if IsSleeping() then
WakeUp;
m_torque := m_torque + torque;
end;
procedure Tb2Body.ApplyImpulse(const impulse, point: TVector2);
begin
if IsSleeping() then
WakeUp;
{$IFDEF OP_OVERLOAD}
m_linearVelocity.AddBy(m_invMass * impulse);
m_angularVelocity := m_angularVelocity + m_invI * b2Cross(point -
m_sweep.c, impulse);
{$ELSE}
AddBy(m_linearVelocity, Multiply(impulse, m_invMass));
m_angularVelocity := m_angularVelocity + m_invI * b2Cross(Subtract(point,
m_sweep.c), impulse);
{$ENDIF}
end;
function Tb2Body.GetWorldPoint(const localPoint: TVector2): TVector2;
begin
Result := b2Mul(m_xf, localPoint);
end;
function Tb2Body.GetWorldVector(const localVector: TVector2): TVector2;
begin
Result := b2Mul(m_xf.R, localVector);
end;
function Tb2Body.GetLocalPoint(const worldPoint: TVector2): TVector2;
begin
Result := b2MulT(m_xf, worldPoint);
end;
function Tb2Body.GetLocalVector(const worldVector: TVector2): TVector2;
begin
Result := b2MulT(m_xf.R, worldVector);
end;
function Tb2Body.GetLinearVelocityFromWorldPoint(
const worldPoint: TVector2): TVector2;
begin
{$IFDEF OP_OVERLOAD}
Result := m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);
{$ELSE}
Result := Add(m_linearVelocity, b2Cross(m_angularVelocity,
Subtract(worldPoint, m_sweep.c)));
{$ENDIF}
end;
function Tb2Body.GetLinearVelocityFromLocalPoint(
const localPoint: TVector2): TVector2;
begin
Result := GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
end;
function Tb2Body.IsBullet: Boolean;
begin
Result := (m_flags and e_bulletFlag) = e_bulletFlag;
end;
procedure Tb2Body.SetBullet(flag: Boolean);
begin
if flag then
m_flags := m_flags or e_bulletFlag
else
m_flags := m_flags and (not e_bulletFlag);
end;
function Tb2Body.IsStatic: Boolean;
begin
Result := m_type = e_staticType;
end;
function Tb2Body.IsDynamic: Boolean;
begin
Result := m_type = e_dynamicType;
end;
function Tb2Body.IsFrozen: Boolean;
begin
Result := (m_flags and e_frozenFlag) = e_frozenFlag;
end;
function Tb2Body.IsSleeping: Boolean;
begin
Result := (m_flags and e_sleepFlag) = e_sleepFlag;
end;
procedure Tb2Body.AllowSleeping(flag: Boolean);
begin
if flag then
m_flags := m_flags or e_allowSleepFlag
else
begin
m_flags := m_flags and (not e_allowSleepFlag);
WakeUp;
end;
end;
procedure Tb2Body.WakeUp;
begin
m_flags := m_flags and (not e_sleepFlag);
m_sleepTime := 0.0;
end;
procedure Tb2Body.PutToSleep;
begin
m_flags := m_flags or e_sleepFlag;
m_sleepTime := 0.0;
SetZero(m_linearVelocity);
m_angularVelocity := 0.0;
SetZero(m_force);
m_torque := 0.0;
end;
///////////////////////////////////////////////
// Specific implementations
procedure b2CollideCircles(var manifold: Tb2Manifold;
circle1, circle2: Tb2CircleShape; const xf1, xf2: Tb2XForm);
var
p1, p2, d: TVector2;
distSqr, r1, r2, radiusSum, separation, dist, a: Float;
begin
manifold.pointCount := 0;
p1 := b2Mul(xf1, circle1.m_localPosition);
p2 := b2Mul(xf2, circle2.m_localPosition);
{$IFDEF OP_OVERLOAD}
d := p2 - p1;
{$ELSE}
d := Subtract(p2, p1);
{$ENDIF}
distSqr := b2Dot(d, d);
r1 := circle1.GetRadius;
r2 := circle2.GetRadius;
radiusSum := r1 + r2;
if distSqr > radiusSum * radiusSum then
Exit;
if (distSqr < FLT_EPSILON) then
begin
separation := -radiusSum;
{$IFDEF OP_OVERLOAD}
manifold.normal.SetValue(0.0, 1.0);
{$ELSE}
SetValue(manifold.normal, 0.0, 1.0);
{$ENDIF}
end
else
begin
dist := Sqrt(distSqr);
separation := dist - radiusSum;
a := 1.0 / dist;
manifold.normal.x := a * d.x;
manifold.normal.y := a * d.y;
end;
manifold.pointCount := 1;
manifold.points[0].id.key := 0;
manifold.points[0].separation := separation;
{$IFDEF OP_OVERLOAD}
p1.AddBy(r1 * manifold.normal);
p2.SubtractBy(r2 * manifold.normal);
d := 0.5 * (p1 + p2);
{$ELSE}
AddBy(p1, Multiply(manifold.normal, r1));
SubtractBy(p2, Multiply(manifold.normal, r2));
d := Add(p1, p2);
MultiplyBy(d, 0.5);
{$ENDIF}
manifold.points[0].localPoint1 := b2MulT(xf1, d);
manifold.points[0].localPoint2 := b2MulT(xf2, d);
end;
procedure b2CollidePolygonAndCircle(var manifold: Tb2Manifold;
polygon: Tb2PolygonShape; circle: Tb2CircleShape; const xf1, xf2: Tb2XForm);
var
i: Integer;
c, cLocal, position, e, p, d: TVector2;
normalIndex: Int32;
_separation, radius, length, dist: Float;
s, u: Float;
vertIndex1, vertIndex2: Int32;
begin
manifold.pointCount := 0;
// Compute circle position in the frame of the polygon.
c := b2Mul(xf2, circle.m_localPosition);
cLocal := b2MulT(xf1, c);
// Find the min separating edge.
normalIndex := 0;
_separation := -FLT_MAX;
radius := circle.GetRadius;
for i := 0 to polygon.GetVertexCount - 1 do
begin
{$IFDEF OP_OVERLOAD}
s := b2Dot(polygon.m_normals[i], cLocal - polygon.m_vertices[i]);
{$ELSE}
s := b2Dot(polygon.m_normals[i], Subtract(cLocal, polygon.m_vertices[i]));
{$ENDIF}
if s > radius then
Exit;
if s > _separation then
begin
_separation := s;
normalIndex := i;
end;
end;
// If the center is inside the polygon ...
if _separation < FLT_EPSILON then
with manifold do
begin
pointCount := 1;
normal := b2Mul(xf1.R, polygon.m_normals[normalIndex]);
with points[0], id do
begin
incidentEdge := normalIndex;
incidentVertex := b2_nullFeature;
referenceEdge := 0;
flip := 0;
{$IFDEF OP_OVERLOAD}
position := c - radius * manifold.normal;
{$ELSE}
position := Subtract(c, Multiply(manifold.normal, radius));
{$ENDIF}
localPoint1 := b2MulT(xf1, position);
localPoint2 := b2MulT(xf2, position);
separation := _separation - radius;
end;
Exit;
end;
// Project the circle center onto the edge segment.
vertIndex1 := normalIndex;
if vertIndex1 + 1 < polygon.GetVertexCount then
vertIndex2 := vertIndex1 + 1
else
vertIndex2 := 0;
{$IFDEF OP_OVERLOAD}
e := polygon.m_vertices[vertIndex2] - polygon.m_vertices[vertIndex1];
length := e.Normalize;
//b2Assert(length > B2_FLT_EPSILON);
// Project the center onto the edge.
u := b2Dot(cLocal - polygon.m_vertices[vertIndex1], e);
{$ELSE}
e := Subtract(polygon.m_vertices[vertIndex2], polygon.m_vertices[vertIndex1]);
length := Normalize(e);
//b2Assert(length > B2_FLT_EPSILON);
// Project the center onto the edge.
u := b2Dot(Subtract(cLocal, polygon.m_vertices[vertIndex1]), e);
{$ENDIF}
if u <= 0.0 then
begin
p := polygon.m_vertices[vertIndex1];
manifold.points[0].id.incidentEdge := b2_nullFeature;
manifold.points[0].id.incidentVertex := vertIndex1;
end
else if u >= length then
begin
p := polygon.m_vertices[vertIndex2];
manifold.points[0].id.incidentEdge := b2_nullFeature;
manifold.points[0].id.incidentVertex := vertIndex2;
end
else
begin
{$IFDEF OP_OVERLOAD}
p := polygon.m_vertices[vertIndex1] + u * e;
{$ELSE}
p := Add(polygon.m_vertices[vertIndex1], Multiply(e, u));
{$ENDIF}
manifold.points[0].id.incidentEdge := normalIndex;
manifold.points[0].id.incidentVertex := 0;
end;
{$IFDEF OP_OVERLOAD}
d := cLocal - p;
dist := d.Normalize;
{$ELSE}
d := Subtract(cLocal, p);
dist := Normalize(d);
{$ENDIF}
if dist > radius then
Exit;
manifold.pointCount := 1;
manifold.normal := b2Mul(xf1.R, d);
{$IFDEF OP_OVERLOAD}
position := c - radius * manifold.normal;
{$ELSE}
position := Subtract(c, Multiply(manifold.normal, radius));
{$ENDIF}
with manifold.points[0] do
begin
localPoint1 := b2MulT(xf1, position);
localPoint2 := b2MulT(xf2, position);
separation := dist - radius;
id.referenceEdge := 0;
id.flip := 0;
end;
end;
type
PClipVertex = ^TClipVertex;
TClipVertex = record
v: TVector2;
id: Tb2ContactID;
end;
PClipVertices = ^TClipVertices;
TClipVertices = array[0..1] of TClipVertex;
function ClipSegmentToLine(vOut, vIn: PClipVertices; const normal: TVector2;
offset: Float): Int32;
var
distance0, distance1, interp: Float;
begin
Result := 0; // Start with no output points
// Calculate the distance of end points to the line
distance0 := b2Dot(normal, vIn^[0].v) - offset;
distance1 := b2Dot(normal, vIn^[1].v) - offset;
// If the points are behind the plane
if distance0 <= 0.0 then
begin
vOut^[Result] := vIn^[0];
Inc(Result);
end;
if distance1 <= 0.0 then
begin
vOut^[Result] := vIn^[1];
Inc(Result);
end;
// If the points are on different sides of the plane
if distance0 * distance1 < 0.0 then
begin
// Find intersection point of edge and plane
interp := distance0 / (distance0 - distance1);
{$IFDEF OP_OVERLOAD}
vOut^[Result].v := vIn^[0].v + interp * (vIn^[1].v - vIn^[0].v);
{$ELSE}
vOut^[Result].v := Add(vIn^[0].v, Multiply(Subtract(vIn^[1].v, vIn^[0].v), interp));
{$ENDIF}
if distance0 > 0.0 then
vOut^[Result].id := vIn^[0].id
else
vOut^[Result].id := vIn^[1].id;
Inc(Result);
end;
end;
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
function EdgeSeparation(poly1, poly2: Tb2PolygonShape; const xf1,
xf2: Tb2XForm; edge1: Int32): Float;
var
i: Integer;
index: Int32;
minDot, dot: Float;
normal1World, normal1: TVector2;
v1, v2: TVector2;
begin
//b2Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
normal1World := b2Mul(xf1.R, poly1.m_normals[edge1]);
normal1 := b2MulT(xf2.R, normal1World);