Delphi/CppBuilder

开发平台：

Delphi

UPhysics2D.pas：源码内容

unit UPhysics2D;
{ This unit is written based on Box2D whose author is Erin Catto (http://www.gphysics.com)
All type names follow the Delphi custom Txxx and xxx means the corresponding
type in cpp source.
Because versions before Delphi 2007 don't support operator overloading, so
I write two versions of all math operations for vector and matrix, etc. But
later I found that the version without operator overloading runs faster.
So if you want a better performance, DEFINE BETTER_PERFORMANCE in Physics2D.inc
which will UNDEFINE OP_OVERLOAD even if you are using Delphi 2010.
This library supports three kinds of floats, Single(32bit), Double(64bit) and
Extended(80bit).
flags EXTENDED_PRECISION DOUBLE_PRECISION
Extended ON whatever
Double(default) OFF ON
Single OFF OFF
There is also a flag SINGLE_PRECISION in the include file but it doesn't affect
Float type definition.
All assertions are ignored.
Translator: Qianyuan Wang(王乾元)
Contact me: http://hi.baidu.com/wqyfavor
wqyfavor@163.com
QQ: 466798985
}
interface
{$I Physics2D.inc}
{$IFDEF D2009UP}
{$POINTERMATH ON}
{$ENDIF}
uses
UPhysics2DTypes,
Math,
Classes,
SysUtils,
Dialogs;
type
RGBA = array[0..3] of Single;
TRGBA = packed record
red, green, blue, alpha: Single;
end;
Tb2ContactID = record
/// The features that intersect to form the contact point
case Integer of
0: (referenceEdge: UInt8; ///< The edge that defines the outward contact normal.
incidentEdge: UInt8; ///< The edge most anti-parallel to the reference edge.
incidentVertex: UInt8; ///< The vertex (0 or 1) on the incident edge that was clipped.
flip: UInt8); ///< A value of 1 indicates that the reference edge is on shape2.
1: (key: UInt32); ///< Used to quickly compare contact ids.
end;
/// A manifold point is a contact point belonging to a contact
/// manifold. It holds details related to the geometry and dynamics
/// of the contact points.
/// The point is stored in local coordinates because CCD
/// requires sub-stepping in which the separation is stale.
Pb2ManifoldPoint = ^Tb2ManifoldPoint;
Tb2ManifoldPoint = record
localPoint1: TVector2; ///< local position of the contact point in body1
localPoint2: TVector2; ///< local position of the contact point in body2
separation: Float; ///< the separation of the shapes along the normal vector
normalImpulse: Float; ///< the non-penetration impulse
tangentImpulse: Float; ///< the friction impulse
id: Tb2ContactID; ///< uniquely identifies a contact point between two shapes
end;
/// A manifold for two touching convex shapes.
Pb2Manifold = ^Tb2Manifold;
Tb2Manifold = record
points: array[0..b2_maxManifoldPoints - 1] of Tb2ManifoldPoint; ///< the points of contact
normal: TVector2; ///< the shared unit normal vector
pointCount: Int32; ///< the number of manifold points
end;
/// A line segment.
Tb2Segment = record
p1, p2: TVector2; // The starting and ending points
{$IFDEF OP_OVERLOAD}
/// Ray cast against this segment with another segment.
function TestSegment(var lambda: Float; var normal: TVector2;
const segment: Tb2Segment; maxLambda: Float): Boolean;
{$ENDIF}
end;
/// An axis aligned bounding box.
Tb2AABB = record
lowerBound, upperBound: TVector2; // The lower and upper vertices
{$IFDEF OP_OVERLOAD}
/// Verify that the bounds are sorted.
function IsValid: Boolean;
{$ENDIF}
end;
/// An oriented bounding box.
Tb2OBB = record
R: TMatrix22; ///< the rotation matrix
center: TVector2; ///< the local centroid
extents: TVector2; ///< the half-widths
end;
Tb2BodyDef = class;
Pb2Body = ^Tb2Body;
Tb2Body = class;
Tb2JointDef = class;
Pb2Joint = ^Tb2Joint;
Tb2Joint = class;
Pb2Contact = ^Tb2Contact;
Tb2Contact = class;
Pb2Shape = ^Tb2Shape;
Tb2Shape = class;
Tb2BroadPhase = class;
Tb2ContactFilter = class;
Tb2ContactListener = class;
Tb2ContactManager = class;
Tb2PairManager = class;
Tb2PairCallback = class;
Tb2Island = class;
Pb2PolyVertices = ^Tb2PolyVertices;
Tb2PolyVertices = array[0..b2_maxPolygonVertices - 1] of TVector2;
//////////////////////////////////////////////////////////////
// World
Tb2TimeStep = record
dt: Float; // time step
inv_dt: Float; // inverse time step (0 if dt == 0).
dtRatio: Float; // dt * inv_dt0
maxIterations: Int32;
warmStarting, positionCorrection: Boolean;
end;
/// Joints and shapes are destroyed when their associated
/// body is destroyed. Implement this listener so that you
/// may nullify references to these joints and shapes.
Tb2DestructionListener = class
public
/// Called when any joint is about to be destroyed due
/// to the destruction of one of its attached bodies.
procedure SayGoodbye(joint: Tb2Joint); overload; virtual; abstract;
/// Called when any shape is about to be destroyed due
/// to the destruction of its parent body.
procedure SayGoodbye(shape: Tb2Shape); overload; virtual; abstract;
end;
/// This is called when a body's shape passes outside of the world boundary.
Tb2BoundaryListener = class
public
/// This is called for each body that leaves the world boundary.
/// @warning you can't modify the world inside this callback.
procedure Violation(body: Tb2Body); virtual; abstract;
end;
Tb2DebugDrawBits = (e_shapeBit, e_jointBit, e_coreShapeBit, e_aabbBit,
e_obbBit, e_pairBit, e_centerOfMassBit);
Tb2DebugDrawBitsSet = set of Tb2DebugDrawBits;
Tb2DebugDraw = class
public
m_drawFlags: Tb2DebugDrawBitsSet;
m_shapeColor_Static, m_shapeColor_Sleeping, m_shapeColor_Normal,
m_pairColor, m_aabbColor, m_obbColor, m_world_aabbColor, m_coreColor,
m_jointLineColor: RGBA;
constructor Create;
procedure DrawPolygon(const vertices: Tb2PolyVertices; vertexCount: Int32; const color: RGBA); virtual; abstract;
procedure DrawPolygon4(const vertices: TVectorArray4; vertexCount: Int32; const color: RGBA); virtual; abstract;
procedure DrawSolidPolygon(const vertices: Tb2PolyVertices; vertexCount: Int32; const color: RGBA); virtual; abstract;
procedure DrawCircle(const center: TVector2; radius: Float; const color: RGBA); virtual; abstract;
procedure DrawSolidCircle(const center, axis: TVector2; radius: Float; const color: RGBA); virtual; abstract;
procedure DrawSegment(const p1, p2: TVector2; const color: RGBA); virtual; abstract;
procedure DrawXForm(const xf: Tb2XForm); virtual; abstract;
end;
/// The world class manages all physics entities, dynamic simulation,
/// and asynchronous queries. The world also contains efficient memory
/// management facilities.
Tb2World = class
private
procedure Solve(const step: Tb2TimeStep);
procedure SolveTOI(const step: Tb2TimeStep);
procedure DrawShape(shape: Tb2Shape; const xf: Tb2XForm; const color: RGBA; core: Boolean);
procedure DrawJoint(joint: Tb2Joint);
public
m_lock: Boolean;
m_broadPhase: Tb2BroadPhase;
m_contactManager: Tb2ContactManager;
m_bodyList: Tb2Body;
m_groundBody: Tb2Body;
m_jointList: Tb2Joint;
// Do not access
m_contactList: Tb2Contact;
m_bodyCount,
m_contactCount,
m_jointCount: Int32;
m_gravity: TVector2;
m_allowSleep: Boolean;
m_destructionListener: Tb2DestructionListener;
m_boundaryListener: Tb2BoundaryListener;
m_contactFilter: Tb2ContactFilter;
m_contactListener: Tb2ContactListener;
m_debugDraw: Tb2DebugDraw;
m_inv_dt0: Float;
m_positionIterationCount: Int32;
m_positionCorrection: Boolean; // This is for debugging the solver.
m_warmStarting: Boolean; // This is for debugging the solver.
m_continuousPhysics: Boolean; // This is for debugging the solver.
/// Construct a world object.
/// @param worldAABB a bounding box that completely encompasses all your shapes.
/// @param gravity the world gravity vector.
/// @param doSleep improve performance by not simulating inactive bodies.
constructor Create(const worldAABB: Tb2AABB; const gravity: TVector2; doSleep: Boolean);
/// Destruct the world. All physics entities are destroyed and all heap memory is released.
destructor Destroy; override;
/// Create a rigid body given a definition. No reference to the definition is retained.
/// @warning This function is locked during callbacks.
function CreateBody(def: Tb2BodyDef; AutoFreeBodyDef: Boolean = True): Tb2Body;
/// Destroy a rigid body given a definition. No reference to the definition
/// is retained. This function is locked during callbacks.
/// @warning This automatically deletes all associated shapes and joints.
/// @warning This function is locked during callbacks.
procedure DestroyBody(body: Tb2Body; DoFree: Boolean = True);
/// Create a joint to constrain bodies together. No reference to the definition
/// is retained. This may cause the connected bodies to cease colliding.
/// @warning This function is locked during callbacks.
function CreateJoint(def: Tb2JointDef; AutoFreeJointDef: Boolean = True): Tb2Joint;
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// @warning This function is locked during callbacks.
procedure DestroyJoint(j: Tb2Joint);
/// Take a time step. This performs collision detection, integration,
/// and constraint solution.
/// @param timeStep the amount of time to simulate, this should not vary.
/// @param iterations the number of iterations to be used by the constraint solver.
procedure Step(timeStep: Float; iterations: Int32; drawThisStep: Boolean = True);
procedure DrawDebugData;
/// Query the world for all shapes that potentially overlap the
/// provided AABB. You provide a shape pointer buffer of specified
/// size. The number of shapes found is returned.
/// @param aabb the query box.
/// @param shapes a user allocated shape pointer array of size maxCount (or greater).
/// @param maxCount the capacity of the shapes array.
/// @return the number of shapes found in aabb.
function Query(const aabb: Tb2AABB; shapes: TList; maxCount: Int32): Int32; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Re-filter a shape. This re-runs contact filtering on a shape.
procedure Refilter(shape: Tb2Shape); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Perform validation of internal data structures.
procedure Validate; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
function GetProxyCount: Int32; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}/// Get the number of broad-phase proxies.
function GetPairCount: Int32; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}/// Get the number of broad-phase pairs.
/// Change the global gravity vector.
procedure SetGravity(const gravity: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
procedure WakeAllSleepingBodies;
//////////////////////////////////////////////////////////////////////
property DestructionListener: Tb2DestructionListener read m_destructionListener write m_destructionListener;
property BoundaryListener: Tb2BoundaryListener read m_boundaryListener write m_boundaryListener;
property ContactFilter: Tb2ContactFilter read m_contactFilter write m_contactFilter;
property ContactListener: Tb2ContactListener read m_contactListener write m_contactListener;
property DebugDraw: Tb2DebugDraw read m_debugDraw write m_debugDraw;
property GetGroundBody: Tb2Body read m_groundBody;
property GetBodyList: Tb2Body read m_bodyList;
property GetJointList: Tb2Joint read m_jointList;
property GetBodyCount: Int32 read m_bodyCount;
property GetJointCount: Int32 read m_jointCount;
property GetContactCount: Int32 read m_contactCount;
property WarmStarting: Boolean read m_warmStarting write m_warmStarting;
property PositionCorrection: Boolean read m_positionCorrection write m_warmStarting;
property ContinuousPhysics: Boolean read m_continuousPhysics write m_continuousPhysics;
end;
////////////////////////////////////////////////////
// Contact
/// A contact edge is used to connect bodies and contacts together
/// in a contact graph where each body is a node and each contact
/// is an edge. A contact edge belongs to a doubly linked list
/// maintained in each attached body. Each contact has two contact
/// nodes, one for each attached body.
Pb2ContactEdge = ^Tb2ContactEdge;
Tb2ContactEdge = record
other: Tb2Body; ///< provides quick access to the other body attached.
contact: Tb2Contact; ///< the contact
prev, next: Pb2ContactEdge;
end;
/// This structure is used to report contact points.
Tb2ContactPoint = record
shape1, shape2: Tb2Shape;
position: TVector2; ///< position in world coordinates
velocity: TVector2; ///< velocity of point on body2 relative to point on body1 (pre-solver)
normal: TVector2; ///< points from shape1 to shape2
separation: Float; ///< the separation is negative when shapes are touching
friction: Float ; ///< the combined friction coefficient
restitution: Float ; ///< the combined restitution coefficient
id: Tb2ContactID; ///< the contact id identifies the features in contact
end;
/// This structure is used to report contact point results.
Tb2ContactResult = record
shape1, shape2: Tb2Shape;
position: TVector2; ///< position in world coordinates
normal: TVector2; ///< points from shape1 to shape2
normalImpulse: Float; ///< the normal impulse applied to body2
tangentImpulse: Float; ///< the tangent impulse applied to body2
id: Tb2ContactID; ///< the contact id identifies the features in contact
end;
Pb2ContactConstraintPoint = ^Tb2ContactConstraintPoint;
Tb2ContactConstraintPoint = record
localAnchor1, localAnchor2: TVector2;
r1, r2: TVector2;
normalImpulse, tangentImpulse, positionImpulse: Float;
normalMass, tangentMass, equalizedMass: Float;
separation, velocityBias: Float;
end;
Pb2ContactConstraint = ^Tb2ContactConstraint;
Tb2ContactConstraint = record
points: array[0..b2_maxManifoldPoints - 1] of Tb2ContactConstraintPoint;
normal: TVector2;
manifold: Pb2Manifold;
body1, body2: Tb2Body;
friction, restitution: Float;
pointCount: Int32;
end;
/// Implement this class to provide collision filtering. In other words,
/// you can implement this class if you want finer control over contact creation.
Tb2ContactFilter = class
public
/// Return True if contact calculations should be performed between
/// these two shapes. @warning for performance reasons this is only
/// called when the AABBs begin to overlap.
function ShouldCollide(shape1, shape2: Tb2Shape): Boolean; virtual;
end;
/// Implement this class to get collision results. You can use these results for
/// things like sounds and game logic. You can also get contact results by
/// traversing the contact lists after the time step. However, you might miss
/// some contacts because continuous physics leads to sub-stepping.
/// Additionally you may receive multiple callbacks for the same contact in a
/// single time step.
/// You should strive to make your callbacks efficient because there may be
/// many callbacks per time step.
/// @warning The contact separation is the last computed value.
/// @warning You cannot create/destroy Box2D entities inside these callbacks.
Tb2ContactListener = class
public
/// Called when a contact point is added. This includes the geometry and the forces.
procedure Add(var point: Tb2ContactPoint); virtual;
/// Called when a contact point persists. This includes the geometry and the forces.
procedure Persist(var point: Tb2ContactPoint); virtual;
/// Called when a contact point is removed. This includes the last
/// computed geometry and forces.
procedure Remove(var point: Tb2ContactPoint); virtual;
/// Called after a contact point is solved.
procedure Result(var point: Tb2ContactResult); virtual;
end;
/// The class manages contact between two shapes. A contact exists for each overlapping
/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
/// that has no contact points.
Tb2ContactClass = class of Tb2Contact;
Tb2Contact = class
public
m_flags: UInt32;
/// The number of manifolds. This is 0 or 1 between convex shapes.
/// This may be greater than 1 for convex-vs-concave shapes. Each
/// manifold holds up to two contact points with a shared contact normal.
m_manifoldCount: Int32;
m_prev, m_next: Tb2Contact; // World pool and list pointers.
m_node1, m_node2: Tb2ContactEdge; // Nodes for connecting bodies.
m_shape1, m_shape2: Tb2Shape;
// Combined friction
m_friction: Float;
m_restitution: Float;
m_toi: Float;
constructor Create; overload;
constructor Create(shape1, shape2: Tb2Shape); overload; virtual;
class function CreateContact(Shape1, Shape2: Tb2Shape): Tb2Contact;
procedure Update(listener: Tb2ContactListener);
procedure Evaluate(listener: Tb2ContactListener); virtual; abstract;
/// Get the manifold array.
function GetManifolds: Pb2Manifold; virtual; abstract;
/// @return True if this contact should generate a response.
function IsSolid: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
property GetManifoldCount: Int32 read m_manifoldCount;
property GetShape1: Tb2Shape read m_shape1;
property GetShape2: Tb2Shape read m_shape2;
property GetNext: Tb2Contact read m_next;
end;
Tb2ContactSolver = class
public
m_step: Tb2TimeStep;
m_constraints: Pb2ContactConstraint;
m_constraintCount: Int32;
constructor Create(const step: Tb2TimeStep; contacts: TList; contactCount: Int32);
destructor Destroy; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep);
procedure SolveVelocityConstraints;
procedure FinalizeVelocityConstraints;
function SolvePositionConstraints(baumgarte: Float): Boolean;
end;
Tb2NullContact = class(Tb2Contact)
public
procedure Evaluate(listener: Tb2ContactListener); override;
function GetManifolds(): Pb2Manifold; override;
end;
Tb2PairCallback = class
public
// This should return the new pair user data. It is ok if the user data is null.
function PairAdded(proxyUserData1, proxyUserData2: Pointer): Pointer; virtual; abstract;
// This should free the pair's user data. In extreme circumstances, it is possible
// this will be called with null pairUserData because the pair never existed.
procedure PairRemoved(proxyUserData1, proxyUserData2, pairUserData: Pointer); virtual; abstract;
end;
// Delegate of b2World.
Tb2ContactManager = class(Tb2PairCallback)
public
m_world: Tb2World;
// This lets us provide broadphase proxy pair user data for
// contacts that shouldn't exist.
m_nullContact: Tb2NullContact;
m_destroyImmediate: Boolean;
constructor Create(world: Tb2World);
// Implements PairCallback
function PairAdded(proxyUserData1, proxyUserData2: Pointer): Pointer; override;
// Implements PairCallback
procedure PairRemoved(proxyUserData1, proxyUserData2, pairUserData: Pointer); override;
procedure Destroy(c: Tb2Contact);
procedure Collide;
end;
////////////////////////////////////////////////////
// Island
Tb2Island = class
public
m_listener: Tb2ContactListener;
m_bodies: TList;
m_contacts: TList;
m_joints: TList;
m_bodyCount: Int32;
m_jointCount: Int32;
m_contactCount: Int32;
m_bodyCapacity, m_contactCapacity, m_jointCapacity: Int32;
m_positionIterationCount: Int32;
constructor Create(bodyCapacity, contactCapacity, jointCapacity: Int32;
listener: Tb2ContactListener);
destructor Destroy; override;
procedure Clear;
procedure Solve(const step: Tb2TimeStep; const gravity: TVector2;
correctPositions, allowSleep: Boolean);
procedure SolveTOI(const subStep: Tb2TimeStep);
procedure Add(body: Tb2Body); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
procedure Add(contact: Tb2Contact); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
procedure Add(joint: Tb2Joint); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
procedure Report(constraints: Pb2ContactConstraint);
end;
////////////////////////////////////////////////////
//
Pb2Bound = ^Tb2Bound;
Tb2Bound = record
value, proxyId, stabbingCount: UInt16;
{$IFDEF OP_OVERLOAD}
function IsLower: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
function IsUpper: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
{$ENDIF}
end;
Pb2Proxy = ^Tb2Proxy;
Tb2Proxy = record
lowerBounds, upperBounds: array[0..1] of UInt16;
overlapCount: UInt16;
timeStamp: UInt16;
userData: Pointer;
{$IFDEF OP_OVERLOAD}
function GetNext: UInt16; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
procedure SetNext(Next: UInt16); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
function IsValid: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
{$ENDIF}
end;
Tb2BoundValuesArray = array[0..1] of UInt16;
Tb2BoundValues = record
lowerValues, upperValues: Tb2BoundValuesArray;
end;
Pb2AxialBoundsArray = ^Tb2AxialBoundsArray;
Tb2AxialBoundsArray = array[0..2 * b2_maxProxies - 1] of Tb2Bound;
Tb2BroadPhase = class
private
procedure ComputeBounds(var lowerValues, upperValues: Tb2BoundValuesArray; const aabb: Tb2AABB);
function TestOverlap(var p1, p2: Tb2Proxy): Boolean; overload;
function TestOverlap(const b: Tb2BoundValues; var p: Tb2Proxy): Boolean; overload;
procedure Query(var lowerIndex, upperIndex: Int32; lowerValue,
upperValue: UInt16; var bounds: Tb2AxialBoundsArray; boundCount, axis: Int32); overload;
procedure IncrementOverlapCount(proxyId: Int32);
procedure IncrementTimeStamp;
public
m_pairManager: Tb2PairManager;
m_proxyPool: array[0..b2_maxProxies - 1] of Tb2Proxy;
m_freeProxy: UInt16;
m_bounds: array[0..1] of Tb2AxialBoundsArray;
m_queryResults: array[0..b2_maxProxies - 1] of UInt16;
m_queryResultCount: Int32;
m_worldAABB: Tb2AABB;
m_quantizationFactor: TVector2;
m_proxyCount: Int32;
m_timeStamp: UInt16;
constructor Create(const worldAABB: Tb2AABB; callback: Tb2PairCallback);
destructor Destroy; override;
// Use this to see if your proxy is in range. If it is not in range,
// it should be destroyed. Otherwise you may get O(m^2) pairs, where m
// is the number of proxies that are out of range.
function InRange(const aabb: Tb2AABB): Boolean;
// Create and destroy proxies. These call Flush first.
function CreateProxy(const aabb: Tb2AABB; userData: Pointer): UInt16;
procedure DestroyProxy(proxyId: Int32);
// Call MoveProxy as many times as you like, then when you are done
// call Commit to finalized the proxy pairs (for your time step).
procedure MoveProxy(proxyId: Int32; const aabb: Tb2AABB);
procedure Commit; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
// Get a single proxy. Returns NULL if the id is invalid.
function GetProxy(proxyId: Int32): Pb2Proxy;
// Query an AABB for overlapping proxies, returns the user data and
// the count, up to the supplied maximum count.
function Query(const aabb: Tb2AABB; userData: TList;
maxCount: Int32): Int32; overload;
procedure Validate;
procedure ValidatePairs;
{$IFDEF CLASSVAR_AVAIL}
class var
s_validate: Boolean;
{$ENDIF}
end;
////////////////////////////////////////////////////
// Pair
Pb2Pair = ^Tb2Pair;
Tb2Pair = record
userData: Pointer;
proxyId1: UInt16;
proxyId2: UInt16;
next: UInt16;
status: UInt16;
{$IFDEF OP_OVERLOAD}
procedure SetBuffered; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { status |= e_pairBuffered; }
procedure ClearBuffered; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { status &= ~e_pairBuffered; }
function IsBuffered: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { return (status & e_pairBuffered) == e_pairBuffered; }
procedure SetRemoved; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { status |= e_pairRemoved; }
procedure ClearRemoved; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { status &= ~e_pairRemoved; }
function IsRemoved: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { return (status & e_pairRemoved) == e_pairRemoved; }
procedure SetFinal; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { status |= e_pairFinal; }
function IsFinal: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} { return (status & e_pairFinal) == e_pairFinal; }
{$ENDIF}
end;
Tb2BufferedPair = record
proxyId1: UInt16;
proxyId2: UInt16;
end;
Tb2PairManager = class
private
function Find(proxyId1, proxyId2: Int32): Pb2Pair; overload;
function Find(proxyId1, proxyId2: Int32; hashValue: UInt32): Pb2Pair; overload;
function AddPair(proxyId1, proxyId2: Int32): Pb2Pair; // Returns existing pair or creates a new one.
function RemovePair(proxyId1, proxyId2: Int32): Pointer;
procedure ValidateBuffer;
procedure ValidateTable;
public
m_broadPhase: Tb2BroadPhase;
m_callback: Tb2PairCallback;
m_pairs: array[0..b2_maxPairs - 1] of Tb2Pair;
m_freePair: UInt16;
m_pairCount: Int32;
m_pairBuffer: array[0..b2_maxPairs - 1] of Tb2BufferedPair;
m_pairBufferCount: Int32;
m_hashTable: array[0..b2_tableCapacity - 1] of UInt16;
constructor Create;
procedure Initialize(broadPhase: Tb2BroadPhase; callback: Tb2PairCallback);
procedure AddBufferedPair(proxyId1, proxyId2: Int32);
procedure RemoveBufferedPair(proxyId1, proxyId2: Int32);
procedure Commit;
end;
//////////////////////////////////////////////////////////////
// Shapes
/// This holds the mass data computed for a shape.
Tb2MassData = record
mass: Float; /// The mass of the shape, usually in kilograms.
I: Float; /// The rotational inertia of the shape.
center: TVector2; /// The position of the shape's centroid relative to the shape's origin.
end;
/// This holds contact filtering data.
Pb2FilterData = ^Tb2FilterData;
Tb2FilterData = record
categoryBits: UInt16; /// The collision category bits. Normally you would just set one bit.
/// The collision mask bits. This states the categories that this
/// shape would accept for collision.
maskBits: UInt16;
/// Collision groups allow a certain group of objects to never collide (negative)
/// or always collide (positive). Zero means no collision group. Non-zero group
/// filtering always wins against the mask bits.
groupIndex: Int16;
end;
/// The various collision shape types supported by Box2D.
Tb2ShapeType = (e_unknownShape = -1, e_circleShape, e_polygonShape);
/// A shape definition is used to construct a shape. This class defines an
/// abstract shape definition. You can reuse shape definitions safely.
Tb2ShapeDef = class
public
ShapeType: Tb2ShapeType; /// Holds the shape type for down-casting.
userData: Pointer; /// Use this to store application specify shape data.
friction: Float; /// The shape's friction coefficient, usually in the range [0,1].
restitution: Float; /// The shape's restitution (elasticity) usually in the range [0,1].
density: Float; /// The shape's density, usually in kg/m^2.
isSensor: Boolean; /// A sensor shape collects contact information but never generates a collision response.
filter: Tb2FilterData; /// Contact filtering data.
constructor Create;
end;
/// A shape is used for collision detection. Shapes are created in b2World.
/// You can use shape for collision detection before they are attached to the world.
/// @warning you cannot reuse shapes.
Tb2Shape = class
private
destructor Destroy2;
protected
procedure CreateProxy(broadPhase: Tb2BroadPhase; const xf: Tb2XForm);
procedure DestroyProxy(broadPhase: Tb2BroadPhase);
function Synchronize(broadPhase: Tb2BroadPhase; const xf1, xf2: Tb2XForm): Boolean;
procedure RefilterProxy(broadPhase: Tb2BroadPhase; const xf: Tb2XForm);
procedure UpdateSweepRadius(const center: TVector2); virtual; abstract;
public
m_type: Tb2ShapeType;
m_next: Tb2Shape;
m_body: Tb2Body;
// Sweep radius relative to the parent body's center of mass.
m_sweepRadius: Float;
m_density: Float;
m_friction: Float;
m_restitution: Float;
m_proxyId: UInt16;
m_filter: Tb2FilterData;
m_isSensor: Boolean;
m_userData: Pointer;
constructor Create(def: Tb2ShapeDef);
destructor Destroy; override;
/// Test a point for containment in this shape. This only works for convex shapes.
/// @param xf the shape world transform.
/// @param p a point in world coordinates.
function TestPoint(const xf: Tb2XForm; const p: TVector2): Boolean; virtual; abstract;
/// Perform a ray cast against this shape.
/// @param xf the shape world transform.
/// @param lambda returns the hit fraction. You can use this to compute
/// the contact point p = (1 - lambda) * segment.p1 + lambda * segment.p2.
/// @param normal returns the normal at the contact point. If there is no
/// intersection, the normal is not set.
/// @param segment defines the begin and end point of the ray cast.
/// @param maxLambda a number typically in the range [0,1].
/// @return True if there was an intersection.
function TestSegment(const xf: Tb2XForm; var lambda: Float; var normal: TVector2;
const segment: Tb2Segment; maxLambda: Float): Boolean; virtual; abstract;
/// Given a transform, compute the associated axis aligned bounding box for this shape.
/// @param aabb returns the axis aligned box.
/// @param xf the world transform of the shape.
procedure ComputeAABB(var aabb: Tb2AABB; const xf: Tb2XForm); virtual; abstract;
/// Given two transforms, compute the associated swept axis aligned bounding box for this shape.
/// @param aabb returns the axis aligned box.
/// @param xf1 the starting shape world transform.
/// @param xf2 the ending shape world transform.
procedure ComputeSweptAABB(var aabb: Tb2AABB; const xf1, xf2: Tb2XForm); virtual; abstract;
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// @param massData returns the mass data for this shape.
procedure ComputeMass(var massData: Tb2MassData); virtual; abstract;
property GetType: Tb2ShapeType read m_type;
property GetBody: Tb2Body read m_body;
property GetFriction: Float read m_friction;
property GetRestitution: Float read m_restitution;
property GetSweepRadius: Float read m_sweepRadius;
property IsSensor: Boolean read m_isSensor write m_isSensor;
end;
//////////////////////////////////////////////////////////////
// Joints
Tb2JointType = (e_unknownJoint, e_revoluteJoint, e_prismaticJoint,
e_distanceJoint, e_pulleyJoint, e_mouseJoint, e_gearJoint, e_fixedJoint);
Tb2LimitState = (e_inactiveLimit, e_atLowerLimit, e_atUpperLimit, e_equalLimits);
Tb2Jacobian = record
linear1, linear2: TVector2;
angular1, angular2: Float;
{$IFDEF OP_OVERLOAD}
procedure SetZero;
procedure SetValue(const x1, x2: TVector2; a1, a2: Float);
function Compute(const x1, x2: TVector2; a1, a2: Float): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
{$ENDIF}
end;
/// A joint edge is used to connect bodies and joints together
/// in a joint graph where each body is a node and each joint
/// is an edge. A joint edge belongs to a doubly linked list
/// maintained in each attached body. Each joint has two joint
/// nodes, one for each attached body.
Pb2JointEdge = ^Tb2JointEdge;
Tb2JointEdge = record
other: Tb2Body; ///< provides quick access to the other body attached.
joint: Tb2Joint; ///< the joint
prev, next: Pb2JointEdge;
end;
/// Joint definitions are used to construct joints.
Tb2JointDef = class
public
JointType: Tb2JointType; /// The joint type is set automatically for concrete joint types.
userData: Pointer; /// Use this to attach application specific data to your joints.
body1, body2: Tb2Body ; /// The attached bodies.
collideConnected: Boolean; /// Set this flag to True if the attached bodies should collide.
constructor Create;
end;
/// The base joint class. Joints are used to constraint two bodies together in
/// various fashions. Some joints also feature limits and motors.
Tb2Joint = class
protected
procedure InitVelocityConstraints(const step: Tb2TimeStep); virtual; abstract;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); virtual; abstract;
// This returns True if the position errors are within tolerance.
procedure InitPositionConstraints; virtual;
function SolvePositionConstraints: Boolean; virtual; abstract;
public
m_type: Tb2JointType;
m_prev, m_next: Tb2Joint;
m_node1, m_node2: Tb2JointEdge;
m_body1, m_body2: Tb2Body;
m_inv_dt: Float;
m_islandFlag, m_collideConnected: Boolean;
m_userData: Pointer;
constructor Create(def: Tb2JointDef);
/// Get the anchor point on body1 in world coordinates.
function GetAnchor1: TVector2; virtual; abstract;
/// Get the anchor point on body2 in world coordinates.
function GetAnchor2: TVector2; virtual; abstract;
/// Get the reaction force on body2 at the joint anchor.
function GetReactionForce: TVector2; virtual; abstract;
/// Get the reaction torque on body2.
function GetReactionTorque: Float; virtual; abstract;
property GetType: Tb2JointType read m_type;
property GetBody1: Tb2Body read m_body1;
property GetBody2: Tb2Body read m_body2;
end;
//////////////////////////////////////////////////////////////
// Body
/// A body definition holds all the data needed to construct a rigid body.
/// You can safely re-use body definitions.
Tb2BodyDef = class
public
/// You can use this to initialized the mass properties of the body.
/// If you prefer, you can set the mass properties after the shapes
/// have been added using b2Body::SetMassFromShapes.
massData: Tb2MassData;
userData: Pointer; /// Use this to store application specific body data.
/// The world position of the body. Avoid creating bodies at the origin
/// since this can lead to many overlapping shapes.
position: TVector2;
angle: Float; // The world angle of the body in radians.
/// Linear damping is use to reduce the linear velocity. The damping parameter
/// can be larger than 1.0f but the damping effect becomes sensitive to the
/// time step when the damping parameter is large.
linearDamping: Float;
/// Angular damping is use to reduce the angular velocity. The damping parameter
/// can be larger than 1.0f but the damping effect becomes sensitive to the
/// time step when the damping parameter is large.
angularDamping: Float;
/// Set this flag to false if this body should never fall asleep. Note that
/// this increases CPU usage.
allowSleep: Boolean;
isSleeping: Boolean; /// Is this body initially sleeping?
fixedRotation: Boolean; /// Should this body be prevented from rotating? Useful for characters.
/// Is this a fast moving body that should be prevented from tunneling through
/// other moving bodies? Note that all bodies are prevented from tunneling through
/// static bodies.
/// @warning You should use this flag sparingly since it increases processing time.
isBullet: Boolean;
constructor Create;
end;
/// A rigid body.
Tb2BodyType = (e_staticType, e_dynamicType, e_maxTypes);
Tb2Body = class
private
destructor Destroy2; // Only free heap
function SynchronizeShapes: Boolean;
procedure SynchronizeTransform;
// This is used to prevent connected bodies from colliding.
// It may lie, depending on the collideConnected flag.
function IsConnected(other: Tb2Body): Boolean;
procedure Advance(t: Float);
public
m_flags: UInt16;
m_type: Tb2BodyType;
m_xf: Tb2XForm; // the body origin transform
m_sweep: Tb2Sweep; // the swept motion for CCD
m_linearVelocity: TVector2;
m_angularVelocity: Float;
m_force: TVector2;
m_torque: Float;
m_world: Tb2World;
m_prev, m_next: Tb2Body;
m_shapeList: Tb2Shape;
m_shapeCount: Int32;
m_jointList: Pb2JointEdge;
m_contactList: Pb2ContactEdge;
m_mass, m_invMass: Float;
m_I, m_invI: Float;
m_linearDamping: Float;
m_angularDamping: Float;
m_sleepTime: Float;
m_userData: Pointer;
constructor Create(bd: Tb2BodyDef; world: Tb2World);
destructor Destroy; override;
/// Creates a shape and attach it to this body.
/// @param shapeDef the shape definition.
/// @warning This function is locked during callbacks.
function CreateShape(shapeDef: Tb2ShapeDef; AutoFreeShapeDef: Boolean = True): Tb2Shape;
/// Destroy a shape. This removes the shape from the broad-phase and
/// therefore destroys any contacts associated with this shape. All shapes
/// attached to a body are implicitly destroyed when the body is destroyed.
/// @param shape the shape to be removed.
/// @warning This function is locked during callbacks.
procedure DestroyShape(s: Tb2Shape; DoFree: Boolean = True);
/// Set the mass properties. Note that this changes the center of mass position.
/// If you are not sure how to compute mass properties, use SetMassFromShapes.
/// The inertia tensor is assumed to be relative to the center of mass.
/// @param massData the mass properties.
procedure SetMass(const massData: Tb2MassData);
/// Compute the mass properties from the attached shapes. You typically call this
/// after adding all the shapes. If you add or remove shapes later, you may want
/// to call this again. Note that this changes the center of mass position.
procedure SetMassFromShapes;
/// Set the position of the body's origin and rotation (radians).
/// This breaks any contacts and wakes the other bodies.
/// @param position the new world position of the body's origin (not necessarily
/// the center of mass).
/// @param angle the new world rotation angle of the body in radians.
/// @return false if the movement put a shape outside the world. In this case the
/// body is automatically frozen.
function SetXForm(const position: TVector2; angle: Float): Boolean;
/// Set the linear velocity of the center of mass.
/// @param v the new linear velocity of the center of mass.
procedure SetLinearVelocity(const v: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Set the angular velocity.
/// @param omega the new angular velocity in radians/second.
procedure SetAngularVelocity(omega: Float); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Apply a force at a world point. If the force is not
/// applied at the center of mass, it will generate a torque and
/// affect the angular velocity. This wakes up the body.
/// @param force the world force vector, usually in Newtons (N).
/// @param point the world position of the point of application.
procedure ApplyForce(const force, point: TVector2);
/// Apply a torque. This affects the angular velocity
/// without affecting the linear velocity of the center of mass.
/// This wakes up the body.
/// @param torque about the z-axis (out of the screen), usually in N-m.
procedure ApplyTorque(torque: Float);
/// Apply an impulse at a point. This immediately modifies the velocity.
/// It also modifies the angular velocity if the point of application
/// is not at the center of mass. This wakes up the body.
/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
/// @param point the world position of the point of application.
procedure ApplyImpulse(const impulse, point: TVector2);
/// Get the world coordinates of a point given the local coordinates.
/// @param localPoint a point on the body measured relative the the body's origin.
/// @return the same point expressed in world coordinates.
function GetWorldPoint(const localPoint: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Get the world coordinates of a vector given the local coordinates.
/// @param localVector a vector fixed in the body.
/// @return the same vector expressed in world coordinates.
function GetWorldVector(const localVector: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Gets a local point relative to the body's origin given a world point.
/// @param a point in world coordinates.
/// @return the corresponding local point relative to the body's origin.
function GetLocalPoint(const worldPoint: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Gets a local vector given a world vector.
/// @param a vector in world coordinates.
/// @return the corresponding local vector.
function GetLocalVector(const worldVector: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Get the world linear velocity of a world point attached to this body.
/// @param a point in world coordinates.
/// @return the world velocity of a point.
function GetLinearVelocityFromWorldPoint(const worldPoint: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Get the world velocity of a local point.
/// @param a point in local coordinates.
/// @return the world velocity of a point.
function GetLinearVelocityFromLocalPoint(const localPoint: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Is this body treated like a bullet for continuous collision detection?
function IsBullet: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Should this body be treated like a bullet for continuous collision detection?
procedure SetBullet(flag: Boolean);
/// Is this body static (immovable)?
function IsStatic: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Is this body dynamic (movable)?
function IsDynamic: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Is this body frozen?
function IsFrozen: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// Is this body sleeping (not simulating).
function IsSleeping: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
/// You can disable sleeping on this body.
procedure AllowSleeping(flag: Boolean);
/// Wake up this body so it will begin simulating.
procedure WakeUp;
/// Put this body to sleep so it will stop simulating.
/// This also sets the velocity to zero.
procedure PutToSleep;
////////////////////////////////////
property GetAngle: Float read m_sweep.a;
property GetPosition: TVector2 read m_xf.position;
property GetWorldCenter: TVector2 read m_sweep.c;
property GetLocalCenter: TVector2 read m_sweep.localCenter;
property GetLinearVelocity: TVector2 read m_linearVelocity;
property GetAngularVelocity: Float read m_angularVelocity;
property GetMass: Float read m_mass;
property GetInertia: Float read m_I;
property GetShapeList: Tb2Shape read m_shapeList;
property GetJointList: Pb2JointEdge read m_jointList;
property GetNext: Tb2Body read m_next;
property UserData: Pointer read m_userData write m_userData;
property GetWorld: Tb2World read m_world;
end;
///////////////////////////////////////////////
// Specific implementations
Tb2CircleDef = class(Tb2ShapeDef)
public
localPosition: TVector2;
radius: Float;
constructor Create;
end;
Tb2CircleShape = class(Tb2Shape)
public
m_radius: Float;
m_localPosition: TVector2; // Local position in parent body
constructor Create(def: Tb2ShapeDef);
procedure UpdateSweepRadius(const center: TVector2); override;
function TestPoint(const transform: Tb2XForm; const p: TVector2): Boolean; override;
function TestSegment(const xf: Tb2XForm; var lambda: Float; var normal: TVector2;
const segment: Tb2Segment; maxLambda: Float): Boolean; override;
procedure ComputeAABB(var aabb: Tb2AABB; const xf: Tb2XForm); override;
procedure ComputeSweptAABB(var aabb: Tb2AABB; const xf1, xf2: Tb2XForm); override;
procedure ComputeMass(var massData: Tb2MassData); override;
/// Get the radius of this circle.
property GetRadius: Float read m_radius;
end;
/// Convex polygon. The vertices must be in CCW order for a right-handed
/// coordinate system with the z-axis coming out of the screen.
Tb2PolygonDef = class(Tb2ShapeDef)
public
/// The polygon vertices in local coordinates.
vertices: Tb2PolyVertices;
vertexCount: Int32;
constructor Create;
/// Build vertices to represent an axis-aligned box.
/// @param hx the half-width.
/// @param hy the half-height.
procedure SetAsBox(hx, hy: Float); overload;
/// Build vertices to represent an oriented box.
/// @param hx the half-width.
/// @param hy the half-height.
/// @param center the center of the box in local coordinates.
/// @param angle the rotation of the box in local coordinates.
procedure SetAsBox(hx, hy: Float; const center: TVector2; angle: Float); overload;
end;
/// A convex polygon.
Tb2PolygonShape = class(Tb2Shape)
private
pm_vertices, pm_normals, pm_coreVertices: PVector2;
public
m_centroid: TVector2; // Local position of the polygon centroid.
m_obb: Tb2OBB; // The oriented bounding box relative to the parent body.
m_vertices: Tb2PolyVertices;
m_normals: Tb2PolyVertices;
m_coreVertices: Tb2PolyVertices;
m_vertexCount: Int32;
constructor Create(const def: Tb2ShapeDef);
procedure UpdateSweepRadius(const center: TVector2); override;
function TestPoint(const transform: Tb2XForm; const p: TVector2): Boolean; override;
function TestSegment(const xf: Tb2XForm; var lambda: Float; var normal: TVector2;
const segment: Tb2Segment; maxLambda: Float): Boolean; override;
procedure ComputeAABB(var aabb: Tb2AABB; const xf: Tb2XForm); override;
procedure ComputeSweptAABB(var aabb: Tb2AABB; const xf1, xf2: Tb2XForm); override;
procedure ComputeMass(var massData: Tb2MassData); override;
/// Get the first vertex and apply the supplied transform.
function GetFirstVertex(const xf: Tb2XForm): TVector2;
/// Get the centroid and apply the supplied transform.
function Centroid(const xf: Tb2XForm): TVector2;
/// Get the support point in the given world direction.
/// Use the supplied transform.
function Support(const xf: Tb2XForm; const d: TVector2): TVector2;
property GetVertices: PVector2 read pm_vertices;
/// Get the core vertices in local coordinates. These vertices
/// represent a smaller polygon that is used for time of impact computations.
property GetCoreVertices: PVector2 read pm_coreVertices;
/// Get the edge normal vectors. There is one for each vertex.
property GetNormals: PVector2 read pm_normals;
property GetVertexCount: Integer read m_vertexCount;
end;
////////////////////////////////////////////////////////////
Tb2CircleContact = class(Tb2Contact)
public
m_manifold: Tb2Manifold;
constructor Create(shape1, shape2: Tb2Shape); override;
procedure Evaluate(listener: Tb2ContactListener); override;
function GetManifolds: Pb2Manifold; override;
end;
Tb2PolyAndCircleContact = class(Tb2Contact)
public
m_manifold: Tb2Manifold;
constructor Create(shape1, shape2: Tb2Shape); override;
procedure Evaluate(listener: Tb2ContactListener); override;
function GetManifolds: Pb2Manifold; override;
end;
Tb2PolygonContact = class(Tb2Contact)
public
m_manifold: Tb2Manifold;
constructor Create(shape1, shape2: Tb2Shape); override;
procedure Evaluate(listener: Tb2ContactListener); override;
function GetManifolds: Pb2Manifold; override;
end;
////////////////////////////////////////////////////////////
/// Distance joint definition. This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. The definition uses local anchor points
/// so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// @warning Do not use a zero or short length.
Tb2DistanceJointDef = class(Tb2JointDef)
public
localAnchor1: TVector2; /// The local anchor point relative to body1's origin.
localAnchor2: TVector2; /// The local anchor point relative to body2's origin.
length : Float; /// The equilibrium length between the anchor points.
frequencyHz: Float; /// The response speed.
dampingRatio: Float; /// The damping ratio. 0 = no damping, 1 = critical damping.
constructor Create;
procedure Initialize(body1, body2: Tb2Body; const anchor1, anchor2: TVector2);
end;
/// A distance joint constrains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
Tb2DistanceJoint = class(Tb2Joint)
public
m_localAnchor1, m_localAnchor2, m_u: TVector2;
m_frequencyHz, m_dampingRatio: Float;
m_gamma, m_bias, m_impulse,
m_mass, // effective mass for the constraint.
m_length: Float;
constructor Create(def: Tb2DistanceJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
end;
/// Prismatic joint definition. This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
Tb2PrismaticJointDef = class(Tb2JointDef)
public
localAnchor1: TVector2;
localAnchor2: TVector2;
localAxis1: TVector2; /// The local translation axis in body1.
referenceAngle: Float; /// The constrained angle between the bodies: body2_angle - body1_angle.
enableLimit: Boolean; /// Enable/disable the joint limit.
lowerTranslation: Float; /// The lower translation limit, usually in meters.
upperTranslation: Float; /// The upper translation limit, usually in meters.
enableMotor: Boolean; /// Enable/disable the joint motor.
maxMotorForce: Float; /// The maximum motor torque, usually in N-m.
motorSpeed: Float; /// The desired motor speed in radians per second.
constructor Create;
procedure Initialize(body1, body2: Tb2Body; const anchor, axis: TVector2); // world anchor and world axis
end;
/// A prismatic joint. This joint provides one degree of freedom: translation
/// along an axis fixed in body1. Relative rotation is prevented. You can
/// use a joint limit to restrict the range of motion and a joint motor to
/// drive the motion or to model joint friction.
Tb2PrismaticJoint = class(Tb2Joint)
public
m_localAnchor1, m_localAnchor2, m_localXAxis1, m_localYAxis1: TVector2;
m_refAngle: Float;
m_linearJacobian: Tb2Jacobian;
m_linearMass: Float; // effective mass for point-to-line constraint.
m_force: Float;
m_angularMass: Float; // effective mass for angular constraint.
m_torque: Float;
m_motorJacobian: Tb2Jacobian;
m_motorMass, // effective mass for motor/limit translational constraint.
m_motorForce,
m_limitForce,
m_limitPositionImpulse: Float;
m_lowerTranslation, m_upperTranslation: Float;
m_maxMotorForce, m_motorSpeed: Float;
m_enableLimit: Boolean;
m_enableMotor: Boolean;
m_limitState: Tb2LimitState;
constructor Create(def: Tb2PrismaticJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
/// Get the current joint translation, usually in meters.
function GetJointTranslation: Float;
/// Get the current joint translation speed, usually in meters per second.
function GetJointSpeed: Float;
/// Set the joint limits, usually in meters.
procedure SetLimits(lower, upper: Float);
property GetMotorSpeed: Float read m_motorSpeed; // usually in meters per second.
property GetMotorForce: Float read m_motorForce; // usually in N.
property LimitEnabled: Boolean read m_enableLimit write m_enableLimit;
property GetLowerLimit: Float read m_lowerTranslation;
property GetUpperLimit: Float read m_upperTranslation;
property MotorEnabled: Boolean read m_enableMotor write m_enableMotor;
property MotorSpeed: Float read m_motorSpeed write m_motorSpeed;
property MaxMotorForce: Float read m_maxMotorForce write m_maxMotorForce;
end;
/// Mouse joint definition. This requires a world target point, tuning parameters, and the time step.
Tb2MouseJointDef = class(Tb2JointDef)
public
/// The initial world target point. This is assumed to coincide with the body anchor initially.
target: TVector2;
/// The maximum constraint force that can be exerted
/// to move the candidate body. Usually you will express
/// as some multiple of the weight (multiplier * mass * gravity).
maxForce: Float;
frequencyHz: Float; /// The response speed.
dampingRatio: Float; /// The damping ratio. 0 = no damping, 1 = critical damping.
timeStep: Float; /// The time step used in the simulation.
constructor Create;
end;
/// A mouse joint is used to make a point on a body track a
/// specified world point. This a soft constraint with a maximum
/// force. This allows the constraint to stretch and without
/// applying huge forces.
Tb2MouseJoint = class(Tb2Joint)
public
m_localAnchor, m_target, m_impulse: TVector2;
m_mass: TMatrix22; // effective mass for point-to-point constraint.
m_C: TVector2; // position error
m_maxForce: Float;
m_beta: Float; // bias factor
m_gamma: Float; // softness
constructor Create(def: Tb2MouseJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
procedure SetTarget(const target: TVector2); /// Use this to update the target point.
end;
/// Pulley joint definition. This requires two ground anchors,
/// two dynamic body anchor points, max lengths for each side,
/// and a pulley ratio.
Tb2PulleyJointDef = class(Tb2JointDef)
public
groundAnchor1: TVector2; /// The first ground anchor in world coordinates. This point never moves.
groundAnchor2: TVector2; /// The second ground anchor in world coordinates. This point never moves.
localAnchor1: TVector2; /// The local anchor point relative to body1's origin.
localAnchor2: TVector2; /// The local anchor point relative to body2's origin.
length1: Float; /// The a reference length for the segment attached to body1.
maxLength1: Float; /// The maximum length of the segment attached to body1.
length2: Float; /// The a reference length for the segment attached to body2.
maxLength2: Float; /// The maximum length of the segment attached to body2.
ratio: Float; /// The pulley ratio, used to simulate a block-and-tackle.
constructor Create;
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
procedure Initialize(body1, body2: Tb2Body; const groundAnchor1, groundAnchor2,
anchor1, anchor2: TVector2; ratio: Float);
end;
/// The pulley joint is connected to two bodies and two fixed ground points.
/// The pulley supports a ratio such that:
/// length1 + ratio * length2 <= constant
/// Yes, the force transmitted is scaled by the ratio.
/// The pulley also enforces a maximum length limit on both sides. This is
/// useful to prevent one side of the pulley hitting the top.
Tb2PulleyJoint = class(Tb2Joint)
public
m_ground: Tb2Body;
m_groundAnchor1, m_groundAnchor2, m_localAnchor1, m_localAnchor2: TVector2;
m_u1, m_u2: TVector2;
m_constant, m_ratio: Float;
m_maxLength1, m_maxLength2: Float;
m_pulleyMass, m_limitMass1, m_limitMass2: Float; // Effective masses
m_force, m_limitForce1, m_limitForce2: Float; // Impulses for accumulation/warm starting.
// Position impulses for accumulation.
m_positionImpulse, m_limitPositionImpulse1, m_limitPositionImpulse2: Float;
m_state, m_limitState1, m_limitState2: Tb2LimitState;
constructor Create(def: Tb2PulleyJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
function GetLength1: Float;
function GetLength2: Float;
function GetGroundAnchor1: TVector2;
function GetGroundAnchor2: TVector2;
property GetRatio: Float read m_ratio;
end;
/// Revolute joint definition. This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
Tb2RevoluteJointDef = class(Tb2JointDef)
public
localAnchor1: TVector2; /// The local anchor point relative to body1's origin.
localAnchor2: TVector2; /// The local anchor point relative to body2's origin.
referenceAngle: Float; /// The body2 angle minus body1 angle in the reference state (radians).
enableLimit: Boolean; /// A flag to enable joint limits.
lowerAngle, upperAngle: Float; /// The lower(upper) angle for the joint limit (radians).
enableMotor: Boolean; /// A flag to enable the joint motor.
motorSpeed: Float; /// The desired motor speed. Usually in radians per second.
maxMotorTorque: Float; /// The maximum motor torque used to achieve the desired motor speed. Usually in N-m.
constructor Create;
/// Initialize the bodies, anchors, and reference angle using the world anchor.
procedure Initialize( body1, body2: Tb2Body; const anchor: TVector2);
end;
/// A revolute joint constrains to bodies to share a common point while they
/// are free to rotate about the point. The relative rotation about the shared
/// point is the joint angle. You can limit the relative rotation with
/// a joint limit that specifies a lower and upper angle. You can use a motor
/// to drive the relative rotation about the shared point. A maximum motor torque
/// is provided so that infinite forces are not generated.
Tb2RevoluteJoint = class(Tb2Joint)
public
m_localAnchor1, m_localAnchor2: TVector2; // relative
m_pivotForce: TVector2;
m_motorForce, m_limitForce, m_limitPositionImpulse: Float;
m_pivotMass: TMatrix22; // effective mass for point-to-point constraint.
m_motorMass: Float; // effective mass for motor/limit angular constraint.
m_enableMotor: Boolean;
m_maxMotorTorque, m_motorSpeed: Float;
m_enableLimit: Boolean;
m_referenceAngle, m_lowerAngle, m_upperAngle: Float;
m_limitState: Tb2LimitState;
constructor Create(def: Tb2RevoluteJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
function GetJointAngle: Float;
function GetJointSpeed: Float; /// Get the current joint angle speed in radians per second.
property LimitEnabled: Boolean read m_enableLimit write m_enableLimit;
property LowerLimit: Float read m_lowerAngle write m_lowerAngle;
property UpperLimit: Float read m_upperAngle write m_upperAngle;
property MotorEnabled: Boolean read m_enableMotor write m_enableMotor;
property MotorSpeed: Float read m_motorSpeed write m_motorSpeed; /// Get the motor speed in radians per second.
property MaxMotorTorque: Float read m_maxMotorTorque write m_maxMotorTorque;
property MotorTorque: Float read m_motorForce write m_motorForce;
end;
/// Gear joint definition. This definition requires two existing
/// revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
Tb2GearJointDef = class(Tb2JointDef)
public
joint1: Tb2Joint; /// The first revolute/prismatic joint attached to the gear joint.
joint2: Tb2Joint; /// The second revolute/prismatic joint attached to the gear joint.
ratio: Float; /// The gear ratio.
constructor Create;
end;
/// A gear joint is used to connect two joints together. Either joint
/// can be a revolute or prismatic joint. You specify a gear ratio
/// to bind the motions together:
/// coordinate1 + ratio * coordinate2 = constant
/// The ratio can be negative or positive. If one joint is a revolute joint
/// and the other joint is a prismatic joint, then the ratio will have units
/// of length or units of 1/length.
/// @warning The revolute and prismatic joints must be attached to
/// fixed bodies (which must be body1 on those joints).
Tb2GearJoint = class(Tb2Joint)
public
m_ground1, m_ground2: Tb2Body;
// One of these is nil.
m_revolute1: Tb2RevoluteJoint;
m_prismatic1: Tb2PrismaticJoint;
// One of these is nil.
m_revolute2: Tb2RevoluteJoint;
m_prismatic2: Tb2PrismaticJoint;
m_groundAnchor1, m_groundAnchor2 :TVector2;
m_localAnchor1, m_localAnchor2: TVector2;
m_J: Tb2Jacobian;
m_constant, m_ratio: Float;
m_mass: Float; // Effective mass
m_force: Float; // Impulse for accumulation/warm starting.
constructor Create(def: Tb2GearJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
property GetRatio: Float read m_ratio;
end;
/// FixedJoint: Attaches two bodies rigidly together
Tb2FixedJointDef = class(Tb2JointDef)
public
constructor Create;
procedure Initialize(body1, body2: Tb2Body); /// Initialize the bodies.
end;
/// A fixed joint constrains all degrees of freedom between two bodies
/// Author: Jorrit Rouwe
/// See: www.jrouwe.nl/fixedjoint/ for more info
Tb2FixedJoint = class(Tb2Joint)
private
procedure CalculateMC; // Get effective constraint mass
public
// Configured state for bodies
m_dp: TVector2; //< Distance between body->GetXForm().position between the two bodies at rest in the reference frame of body1
m_a: Float; //< Angle between the bodies at rest
m_R0: TMatrix22; //< Rotation matrix of m_a
// State for solving
m_inv_dt: Float; //< Stored 1/dt
m_d: TVector2; //< Distance between center of masses for this time step (when the shapes of the bodies change, their local centers can change so we derive this from m_dp every frame)
m_a1: Float; //< Stored angle of body 1 (a1) to determine if it changed
m_c, m_s: Float; //< cos(a1) and sin(a1)
m_Ax, m_Ay: Float; //< A = d/dt (R(a1) d)
m_mc: array[0..2, 0..2] of Float; //< Effective constraint mass
// State after solving
m_lambda: array[0..2] of Float; //< Accumulated lambdas for warm starting and returning constraint force
constructor Create(def: Tb2FixedJointDef);
function GetAnchor1: TVector2; override;
function GetAnchor2: TVector2; override;
function GetReactionForce: TVector2; override;
function GetReactionTorque: Float; override;
procedure InitVelocityConstraints(const step: Tb2TimeStep); override;
procedure SolveVelocityConstraints(const step: Tb2TimeStep); override;
function SolvePositionConstraints: Boolean; override;
end;
//////////////////////////////////
procedure b2CollideCircles(var manifold: Tb2Manifold;
circle1, circle2: Tb2CircleShape; const xf1, xf2: Tb2XForm);
procedure b2CollidePolygonAndCircle(var manifold: Tb2Manifold;
polygon: Tb2PolygonShape; circle: Tb2CircleShape; const xf1, xf2: Tb2XForm);
procedure b2CollidePolygons(var manifold: Tb2Manifold;
polyA, polyB: Tb2PolygonShape; xfA, xfB: Tb2XForm);
function b2TimeOfImpact(shape1, shape2: Tb2Shape; const sweep1, sweep2: Tb2Sweep): Float;
function b2Distance(var x1, x2: TVector2; circle1, circle2: Tb2CircleShape;
const xf1, xf2: Tb2XForm): Float; overload;
function b2Distance(var x1, x2: TVector2; poly: Tb2PolygonShape;
circle: Tb2CircleShape; const xf1, xf2: Tb2XForm): Float; overload;
function b2Distance(var x1, x2: TVector2; circle: Tb2CircleShape;
poly: Tb2PolygonShape; const xf1, xf2: Tb2XForm): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
function b2Distance(var x1, x2: TVector2; poly1, poly2: Tb2PolygonShape;
const xf1, xf2: Tb2XForm): Float; overload;
function b2Distance(var x1, x2: TVector2; shape1, shape2: Tb2Shape;
const xf1, xf2: Tb2XForm): Float; overload;
/////////////////// Color functions //////
function MakeColor(r, g, b: Single; a: Single = 1.0): RGBA;
var
g_GJK_Iterations: Int32;
implementation
var
b2_defaultFilter: Tb2ContactFilter;
{$IFNDEF CLASSVAR_AVAIL}
b2BroadPhase_s_validate: Boolean;
{$ENDIF}
const
// Tb2Pair.status
e_pairBuffered = 1;
e_pairRemoved = 2;
e_pairFinal = 4;
// Tb2Contact.m_flags
e_nonSolidFlag = 1;
e_slowFlag = 2;
e_contact_islandFlag = 4;
e_toiFlag = 8;
// Tb2Body.m_flags
e_frozenFlag = $0002;
e_body_islandFlag = $0004;
e_sleepFlag = $0008;
e_allowSleepFlag = $0010;
e_bulletFlag = $0020;
e_fixedRotationFlag = $0040;
function MakeColor(r, g, b: Single; a: Single = 1.0): RGBA;
begin
Result[0] := r;
Result[1] := g;
Result[2] := b;
Result[3] := a;
end;
//////////// Implements <b2Contact.cpp> InitializeRegisters and AddType
type
TContactCreateRecord = record
ClassType: Tb2ContactClass;
Primary: Boolean;
end;
const
ContactCreateRecords: array[e_circleShape..e_polygonShape,
e_circleShape..e_polygonShape] of TContactCreateRecord = (
((ClassType: Tb2CircleContact; Primary: True),
(ClassType: Tb2PolyAndCircleContact; Primary: False)),
((ClassType: Tb2PolyAndCircleContact; Primary: True),
(ClassType: Tb2PolygonContact; Primary: True)));
{$IFNDEF OP_OVERLOAD}
// Record methods
function TestSegment(const Self: Tb2Segment; var lambda: Float;
var normal: TVector2; const segment: Tb2Segment; maxLambda: Float): Boolean;
const
k_slop = 100.0 * FLT_EPSILON;
var
s, r, d, n, b: TVector2;
denom, a, mu2: Float;
begin
with Self do
begin
s := segment.p1;
r := Subtract(segment.p2, s);
d := Subtract(p2, p1);
n := b2Cross(d, 1.0);
denom := -b2Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if denom > k_slop then
begin
// Does the segment intersect the infinite line associated with this segment?
b := Subtract(s, p1);
a := b2Dot(b, n);
if (0.0 <= a) and (a <= maxLambda * denom) then
begin
mu2 := r.y * b.x - r.x * b.y;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2) and (mu2 <= denom * (1.0 + k_slop)) then
begin
Normalize(n);
lambda := a / denom;
normal := n;
Result := True;
Exit;
end;
end;
end;
end;
Result := False;
end;
function IsValid(const AABB: Tb2AABB): Boolean; overload;
var
d: TVector2;
begin
with AABB do
begin
d := Subtract(upperBound, lowerBound);
Result := (d.x >= 0.0) and (d.y >= 0.0) and
UPhysics2DTypes.IsValid(upperBound) and UPhysics2DTypes.IsValid(lowerBound);
end;
end;
function IsLower(const bound: Tb2Bound): Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with bound do
Result := (value and 1) = 0;
end;
function IsUpper(const bound: Tb2Bound): Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with bound do
Result := (value and 1) = 1;
end;
function GetNext(const proxy: Tb2Proxy): UInt16; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
Result := proxy.lowerBounds[0];
end;
procedure SetNext(var proxy: Tb2Proxy; Next: UInt16); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
proxy.lowerBounds[0] := Next;
end;
function IsValid(const proxy: Tb2Proxy): Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF} overload;
begin
Result := proxy.overlapCount <> b2_invalid;
end;
procedure SetBuffered(var pair: Tb2Pair); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
status := status or e_pairBuffered;
end;
procedure ClearBuffered(var pair: Tb2Pair); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
status := status and (not e_pairBuffered);
end;
function IsBuffered(const pair: Tb2Pair): Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
Result := (status and e_pairBuffered) = e_pairBuffered;
end;
procedure SetRemoved(var pair: Tb2Pair); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
status := status or e_pairRemoved;
end;
procedure ClearRemoved(var pair: Tb2Pair); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
status := status and (not e_pairRemoved);
end;
function IsRemoved(const pair: Tb2Pair): Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
Result := (status and e_pairRemoved) = e_pairRemoved;
end;
procedure SetFinal(var pair: Tb2Pair); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
status := status or e_pairFinal;
end;
function IsFinal(const pair: Tb2Pair): Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with pair do
Result := (status and e_pairFinal) = e_pairFinal;
end;
procedure SetZero(var jb: Tb2Jacobian); overload;
begin
with jb do
begin
SetZero(linear1);
SetZero(linear2);
angular1 := 0.0;
angular2 := 0.0;
end;
end;
procedure SetValue(var jb: Tb2Jacobian; const x1, x2: TVector2; a1, a2: Float); overload;
begin
with jb do
begin
linear1 := x1;
linear2 := x2;
angular1 := a1;
angular2 := a2;
end;
end;
function Compute(var jb: Tb2Jacobian; const x1, x2: TVector2; a1, a2: Float): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
with jb do
Result := b2Dot(linear1, x1) + angular1 * a1 + b2Dot(linear2, x2) + angular2 * a2;
end;
{$ENDIF}
procedure b2Swap(var a, b: Tb2Bound); {$IFDEF INLINE_AVAIL}inline;{$ENDIF} overload;
var
tmp: Tb2Bound;
begin
tmp := a;
a := b;
b := tmp;
end;
{ b2BroadPhase.cpp }
function BroadPhase_BinarySearch(var bounds: Tb2AxialBoundsArray; count: Int32; value: UInt16): Int32;
var
low, high, mid: Int32;
pbm: Pb2Bound;
begin
low := 0;
high := count - 1;
while (low <= high) do
begin
mid := (low + high) shr 1;
pbm := @bounds[mid];
if pbm^.value > value then
high := mid - 1
else if pbm^.value < value then
low := mid + 1
else
begin
Result := mid;
Exit;
end;
end;
Result := low;
end;
{ b2PolygonShape. cpp}
function ComputeCentroid(const vs: Tb2PolyVertices; count: Int32): TVector2;
const
inv3 = 1 / 3;
var
i: Integer;
pRef, p1, p2, p3, e1, e2: TVector2;
area, triangleArea: Float;
begin
//b2Assert(count >= 3);
area := 0.0;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
SetZero(Result);
SetZero(pRef);
(* // This code would put the reference point inside the polygon.
for (int32 i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count; *)
for i := 0 to count - 1 do
begin
// Triangle vertices.
p1 := pRef;
p2 := vs[i];
if i + 1 < count then
p3 := vs[i + 1]
else
p3 := vs[0];
{$IFDEF OP_OVERLOAD}
e1 := p2 - p1;
e2 := p3 - p1;
{$ELSE}
e1 := Subtract(p2, p1);
e2 := Subtract(p3, p1);
{$ENDIF}
triangleArea := 0.5 * b2Cross(e1, e2);
area := area + triangleArea;
// Area weighted centroid
{$IFDEF OP_OVERLOAD}
Result := Result + triangleArea * inv3 * (p1 + p2 + p3);
{$ELSE}
AddBy(p1, p2);
AddBy(p1, p3);
MultiplyBy(p1, triangleArea * inv3);
AddBy(Result, p1);
{$ENDIF}
end;
// Centroid
//b2Assert(area > B2_FLT_EPSILON);
{$IFDEF OP_OVERLOAD}
Result := Result / area;
{$ELSE}
DivideBy(Result, area);
{$ENDIF}
end;
// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
procedure ComputeOBB(var obb: Tb2OBB; const vs: Tb2PolyVertices; count: Int32);
var
i, j: Integer;
p: array[0..b2_maxPolygonVertices] of TVector2;
minArea, length, area: Float;
root, ux, uy, lower, upper, d, r, center: TVector2;
begin
//b2Assert(count <= b2_maxPolygonVertices);
Move(vs, p, SizeOf(vs));
p[count] := p[0];
minArea := FLT_MAX;
for i := 1 to Count - 1 do
begin
root := p[i - 1];
{$IFDEF OP_OVERLOAD}
ux := p[i] - root;
length := ux.Normalize;
//b2Assert(length > B2_FLT_EPSILON);
uy.SetValue(-ux.y, ux.x);
lower.SetValue(FLT_MAX, FLT_MAX);
upper.SetValue(-FLT_MAX, -FLT_MAX);
{$ELSE}
ux := Subtract(p[i], root);
length := Normalize(ux);
//b2Assert(length > B2_FLT_EPSILON);
SetValue(uy, -ux.y, ux.x);
SetValue(lower, FLT_MAX, FLT_MAX);
SetValue(upper, -FLT_MAX, -FLT_MAX);
{$ENDIF}
for j := 0 to Count - 1 do
begin
{$IFDEF OP_OVERLOAD}
d := p[j] - root;
{$ELSE}
d := Subtract(p[j], root);
{$ENDIF}
r.x := b2Dot(ux, d);
r.y := b2Dot(uy, d);
lower := b2Min(lower, r);
upper := b2Max(upper, r);
end;
area := (upper.x - lower.x) * (upper.y - lower.y);
if area < 0.95 * minArea then
begin
minArea := area;
obb.R.col1 := ux;
obb.R.col2 := uy;
{$IFDEF OP_OVERLOAD}
center := 0.5 * (lower + upper);
obb.center := root + b2Mul(obb.R, center);
obb.extents := 0.5 * (upper - lower);
{$ELSE}
center := Add(lower, upper);
MultiplyBy(center, 0.5);
obb.center := Add(root, b2Mul(obb.R, center));
obb.extents := Subtract(upper, lower);
MultiplyBy(obb.extents, 0.5);
{$ENDIF}
end;
end;
//b2Assert(minArea < B2_FLT_MAX);
end;
{ b2Distance.cpp }
// GJK using Voronoi regions (Christer Ericson) and region selection
// optimizations (Casey Muratori).
// The origin is either in the region of points[1] or in the edge region. The origin is
// not in region of points[0] because that is the old point.
type
TCalcDistanceVectors = array[0..2] of TVector2;
function Distance_ProcessTwo(var x1, x2: TVector2; var p1s, p2s,
points: TCalcDistanceVectors): Int32;
var
r, d: TVector2;
length, lambda: Float;
begin
{$IFDEF OP_OVERLOAD}
r := -points[1];
d := points[0] - points[1];
length := d.Normalize;
{$ELSE}
r := Negative(points[1]);
d := Subtract(points[0], points[1]);
length := Normalize(d);
{$ENDIF}
lambda := b2Dot(r, d);
if (lambda <= 0.0) or (length < FLT_EPSILON) then
begin
// The simplex is reduced to a point.
x1 := p1s[1];
x2 := p2s[1];
p1s[0] := p1s[1];
p2s[0] := p2s[1];
points[0] := points[1];
Result := 1;
Exit;
end;
// Else in edge region
lambda := lambda / length;
{$IFDEF OP_OVERLOAD}
x1 := p1s[1] + lambda * (p1s[0] - p1s[1]);
x2 := p2s[1] + lambda * (p2s[0] - p2s[1]);
{$ELSE}
x1 := Subtract(p1s[0], p1s[1]);
MultiplyBy(x1, lambda);
AddBy(x1, p1s[1]);
x2 := Subtract(p2s[0], p2s[1]);
MultiplyBy(x2, lambda);
AddBy(x2, p2s[1]);
{$ENDIF}
Result := 2;
end;
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
function Distance_ProcessThree(var x1, x2: TVector2; var p1s, p2s,
points: TCalcDistanceVectors): Int32;
var
a, b, c, ab, ac, bc: TVector2;
tn, td, un, ud, n, vc, va, lambda, vb, denom: Float;
begin
a := points[0];
b := points[1];
c := points[2];
{$IFDEF OP_OVERLOAD}
ab := b - a;
ac := c - a;
bc := c - b;
{$ELSE}
ab := Subtract(b, a);
ac := Subtract(c, a);
bc := Subtract(c, b);
{$ENDIF}
//float32 sn := -b2Dot(a, ab), sd := b2Dot(b, ab);
tn := -b2Dot(a, ac);
td := b2Dot(c, ac);
un := -b2Dot(b, bc);
ud := b2Dot(c, bc);
// In vertex c region?
if (td <= 0.0) and (ud <= 0.0) then
begin
// Single point
x1 := p1s[2];
x2 := p2s[2];
p1s[0] := p1s[2];
p2s[0] := p2s[2];
points[0] := points[2];
Result := 1;
Exit;
end;
// Should not be in vertex a or b region.
//B2_NOT_USED(sd);
//B2_NOT_USED(sn);
//b2Assert(sn > 0.0 || tn > 0.0);
//b2Assert(sd > 0.0 || un > 0.0);
n := b2Cross(ab, ac);
// Should not be in edge ab region.
vc := n * b2Cross(a, b);
//b2Assert(vc > 0.0 || sn > 0.0 || sd > 0.0);
// In edge bc region?
va := n * b2Cross(b, c);
if (va <= 0.0) and (un >= 0.0) and (ud >= 0.0) and (un + ud > 0.0) then
begin
//b2Assert(un + ud > 0.0);
lambda := un / (un + ud);
{$IFDEF OP_OVERLOAD}
x1 := p1s[1] + lambda * (p1s[2] - p1s[1]);
x2 := p2s[1] + lambda * (p2s[2] - p2s[1]);
{$ELSE}
x1 := Subtract(p1s[2], p1s[1]);
MultiplyBy(x1, lambda);
AddBy(x1, p1s[1]);
x2 := Subtract(p2s[2], p2s[1]);
MultiplyBy(x2, lambda);
AddBy(x2, p2s[1]);
{$ENDIF}
p1s[0] := p1s[2];
p2s[0] := p2s[2];
points[0] := points[2];
Result := 2;
Exit;
end;
// In edge ac region?
vb := n * b2Cross(c, a);
if (vb <= 0.0) and (tn >= 0.0) and (td >= 0.0) and (tn + td > 0.0) then
begin
//b2Assert(tn + td > 0.0);
lambda := tn / (tn + td);
{$IFDEF OP_OVERLOAD}
x1 := p1s[0] + lambda * (p1s[2] - p1s[0]);
x2 := p2s[0] + lambda * (p2s[2] - p2s[0]);
{$ELSE}
x1 := Subtract(p1s[2], p1s[0]);
MultiplyBy(x1, lambda);
AddBy(x1, p1s[0]);
x2 := Subtract(p2s[2], p2s[0]);
MultiplyBy(x2, lambda);
AddBy(x2, p2s[0]);
{$ENDIF}
p1s[1] := p1s[2];
p2s[1] := p2s[2];
points[1] := points[2];
Result := 2;
Exit;
end;
// Inside the triangle, compute barycentric coordinates
denom := va + vb + vc;
//b2Assert(denom > 0.0);
denom := 1.0 / denom;
tn := va * denom;
td := vb * denom;
un := 1.0 - tn - td;
{$IFDEF OP_OVERLOAD}
x1 := tn * p1s[0] + td * p1s[1] + un * p1s[2];
x2 := tn * p2s[0] + td * p2s[1] + un * p2s[2];
{$ELSE}
MultiplyBy(p1s[0], tn);
MultiplyBy(p1s[1], td);
MultiplyBy(p1s[2], un);
x1 := Add(p1s[0], p1s[1], p1s[2]);
MultiplyBy(p2s[0], tn);
MultiplyBy(p2s[1], td);
MultiplyBy(p2s[2], un);
x2 := Add(p2s[0], p2s[1], p2s[2]);
{$ENDIF}
Result := 3;
end;
function Distance_InPoints(const w: TVector2; var points: TCalcDistanceVectors;
pointCount: Int32): Boolean;
const
k_tolerance = 100.0 * FLT_EPSILON;
var
i: Integer;
d, m: TVector2;
begin
for i := 0 to pointCount - 1 do
begin
{$IFDEF OP_OVERLOAD}
d := b2Abs(w - points[i]);
{$ELSE}
d := b2Abs(Subtract(w, points[i]));
{$ENDIF}
m := b2Max(b2Abs(w), b2Abs(points[i]));
if (d.x < k_tolerance * (m.x + 1.0)) and (d.y < k_tolerance * (m.y + 1.0)) then
begin
Result := True;
Exit;
end;
end;
Result := False;
end;
{ b2TimeOfImpact.cpp }
// This algorithm uses conservative advancement to compute the time of
// impact (TOI) of two shapes.
// Refs: Bullet, Young Kim
function b2TimeOfImpact(shape1, shape2: Tb2Shape; const sweep1, sweep2: Tb2Sweep): Float;
var
t0: Float;
omega1, omega2, alpha: Float;
v1, v2, p1, p2, normal: TVector2;
k_maxIterations, iter: Int32;
distance, targetDistance, t, approachVelocityBound, newAlpha: Float;
xf1, xf2: Tb2XForm;
begin
//b2Assert(sweep1.t0 == sweep2.t0);
//b2Assert(1.0 - sweep1.t0 > B2_FLT_EPSILON);
t0 := sweep1.t0;
{$IFDEF OP_OVERLOAD}
v1 := sweep1.c - sweep1.c0;
v2 := sweep2.c - sweep2.c0;
{$ELSE}
v1 := Subtract(sweep1.c, sweep1.c0);
v2 := Subtract(sweep2.c, sweep2.c0);
{$ENDIF}
omega1 := sweep1.a - sweep1.a0;
omega2 := sweep2.a - sweep2.a0;
alpha := 0.0;
k_maxIterations := 20; // TODO_ERIN b2Settings
iter := 0;
normal := b2Vec2_zero;
distance := 0.0;
targetDistance := 0.0;
while True do
begin
t := (1.0 - alpha) * t0 + alpha;
{$IFDEF OP_OVERLOAD}
sweep1.GetXForm(xf1, t);
sweep2.GetXForm(xf2, t);
{$ELSE}
GetXForm(sweep1, xf1, t);
GetXForm(sweep2, xf2, t);
{$ENDIF}
// Get the distance between shapes.
distance := b2Distance(p1, p2, shape1, shape2, xf1, xf2);
if iter = 0 then
begin
// Compute a reasonable target distance to give some breathing room
// for conservative advancement.
if distance > 2.0 * b2_toiSlop then
targetDistance := 1.5 * b2_toiSlop
else
targetDistance := b2Max(0.05 * b2_toiSlop, distance - 0.5 * b2_toiSlop);
end;
if (distance - targetDistance < 0.05 * b2_toiSlop) or (iter = k_maxIterations) then
Break;
{$IFDEF OP_OVERLOAD}
normal := p2 - p1;
normal.Normalize;
{$ELSE}
normal := Subtract(p2, p1);
Normalize(normal);
{$ENDIF}
// Compute upper bound on remaining movement.
{$IFDEF OP_OVERLOAD}
approachVelocityBound := b2Dot(normal, v1 - v2) + Abs(omega1) *
shape1.GetSweepRadius + Abs(omega2) * shape2.GetSweepRadius;
{$ELSE}
approachVelocityBound := b2Dot(normal, Subtract(v1, v2)) + Abs(omega1) *
shape1.GetSweepRadius + Abs(omega2) * shape2.GetSweepRadius;
{$ENDIF}
if Abs(approachVelocityBound) < FLT_EPSILON then
begin
alpha := 1.0;
Break;
end;
// Get the conservative time increment. Don't advance all the way.
newAlpha := alpha + (distance - targetDistance) / approachVelocityBound;
// The shapes may be moving apart or a safe distance apart.
if (newAlpha < 0.0) or (1.0 < newAlpha) then
begin
alpha := 1.0;
Break;
end;
// Ensure significant advancement.
if newAlpha < (1.0 + 100.0 * FLT_EPSILON) * alpha then
Break;
alpha := newAlpha;
Inc(iter);
end;
Result := alpha;
end;
{ b2Distance.cpp }
// Circle to circle
function b2Distance(var x1, x2: TVector2; circle1, circle2: Tb2CircleShape;
const xf1, xf2: Tb2XForm): Float; overload;
var
p1, p2, d: TVector2;
dsqr, r1, r2, r, dLen: Float;
begin
p1 := b2Mul(xf1, circle1.m_localPosition);
p2 := b2Mul(xf2, circle2.m_localPosition);
{$IFDEF OP_OVERLOAD}
d := p2 - p1;
{$ELSE}
d := Subtract(p2, p1);
{$ENDIF}
dSqr := b2Dot(d, d);
r1 := circle1.GetRadius - b2_toiSlop;
r2 := circle2.GetRadius - b2_toiSlop;
r := r1 + r2;
if dSqr > r * r then
begin
{$IFDEF OP_OVERLOAD}
dLen := d.Normalize;
x1 := p1 + r1 * d;
x2 := p2 - r2 * d;
{$ELSE}
dLen := Normalize(d);
x1 := Multiply(d, r1);
AddBy(x1, p1);
x2 := Multiply(d, -r2);
AddBy(x2, p2);
{$ENDIF}
Result := dLen - r;
Exit;
end
else if (dSqr > FLT_EPSILON * FLT_EPSILON) then
begin
{$IFDEF OP_OVERLOAD}
d.Normalize;
x1 := p1 + r1 * d;
{$ELSE}
Normalize(d);
x1 := Multiply(d, r1);
AddBy(x1, p1);
{$ENDIF}
x2 := x1;
Result := 0.0;
Exit;
end;
x1 := p1;
x2 := x1;
Result := 0.0;
end;
// Polygon to circle
const
maxIterations = 20;
function Distance_Generic_Simulate(var x1, x2: TVector2;
poly1: Tb2PolygonShape; poly2: Pointer; const xf1, xf2: Tb2XForm;
poly2AsVector: Boolean): Float;
var
i, iter: Integer;
p1s, p2s, points: TCalcDistanceVectors;
pointCount: Int32;
vSqr, vw, maxSqr: Float;
v, w1, w2, w: TVector2;
begin
pointCount := 0;
x1 := poly1.GetFirstVertex(xf1);
if poly2AsVector then
x2 := PVector2(poly2)^
else
x2 := Tb2PolygonShape(poly2).GetFirstVertex(xf2);
vSqr := 0.0;
for iter := 0 to maxIterations - 1 do
begin
{$IFDEF OP_OVERLOAD}
v := x2 - x1;
{$ELSE}
v := Subtract(x2, x1);
{$ENDIF}
w1 := poly1.Support(xf1, v);
if poly2AsVector then
w2 := PVector2(poly2)^ // Ths same
else
{$IFDEF OP_OVERLOAD}
w2 := Tb2PolygonShape(poly2).Support(xf2, -v);
{$ELSE}
w2 := Tb2PolygonShape(poly2).Support(xf2, Negative(v));
{$ENDIF}
vSqr := b2Dot(v, v);
{$IFDEF OP_OVERLOAD}
w := w2 - w1;
{$ELSE}
w := Subtract(w2, w1);
{$ENDIF}
vw := b2Dot(v, w);
if (vSqr - vw <= 0.01 * vSqr) or Distance_InPoints(w, points, pointCount) then // or w in points
begin
if pointCount = 0 then
begin
x1 := w1;
x2 := w2;
end;
g_GJK_Iterations := iter;
Result := Sqrt(vSqr);
Exit;
end;
case pointCount of
0: begin
p1s[0] := w1;
p2s[0] := w2;
points[0] := w;
x1 := p1s[0];
x2 := p2s[0];
Inc(pointCount);
end;
1: begin
p1s[1] := w1;
p2s[1] := w2;
points[1] := w;
pointCount := Distance_ProcessTwo(x1, x2, p1s, p2s, points);
end;
2: begin
p1s[2] := w1;
p2s[2] := w2;
points[2] := w;
pointCount := Distance_ProcessThree(x1, x2, p1s, p2s, points);
end;
end;
// If we have three points, then the origin is in the corresponding triangle.
if pointCount = 3 then
begin
g_GJK_Iterations := iter;
Result := 0.0;
Exit;
end;
maxSqr := -FLT_MAX;
for i := 0 to pointCount - 1 do
maxSqr := b2Max(maxSqr, b2Dot(points[i], points[i]));
if (pointCount = 3) or (vSqr <= 100.0 * FLT_EPSILON * maxSqr) then
begin
g_GJK_Iterations := iter;
{$IFDEF OP_OVERLOAD}
v := x2 - x1;
{$ELSE}
v := Subtract(x2, x1);
{$ENDIF}
vSqr := b2Dot(v, v);
Result := Sqrt(vSqr);
Exit;
end;
end;
g_GJK_Iterations := maxIterations;
Result := Sqrt(vSqr);
end;
function b2Distance(var x1, x2: TVector2; poly: Tb2PolygonShape;
circle: Tb2CircleShape; const xf1, xf2: Tb2XForm): Float; overload;
var
p: TVector2;
r: Float;
d: TVector2;
begin
p := b2Mul(xf2, circle.m_localPosition);
Result := Distance_Generic_Simulate(x1, x2, poly, Pointer(@p),
xf1, b2XForm_identity, True);
r := circle.GetRadius - b2_toiSlop;
if Result > r then
begin
Result := Result - r;
{$IFDEF OP_OVERLOAD}
d := x2 - x1;
d.Normalize;
x2 := x2 - r * d;
{$ELSE}
d := Subtract(x2, x1);
Normalize(d);
SubtractBy(x2, Multiply(d, r));
{$ENDIF}
end
else
begin
Result := 0.0;
x2 := x1;
end;
end;
function b2Distance(var x1, x2: TVector2; circle: Tb2CircleShape;
poly: Tb2PolygonShape; const xf1, xf2: Tb2XForm): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
Result := b2Distance(x2, x1, poly, circle, xf2, xf1);
end;
function b2Distance(var x1, x2: TVector2; poly1, poly2: Tb2PolygonShape;
const xf1, xf2: Tb2XForm): Float; overload;
begin
Result := Distance_Generic_Simulate(x1, x2, poly1, poly2, xf1, xf2, False);
end;
function b2Distance(var x1, x2: TVector2; shape1, shape2: Tb2Shape;
const xf1, xf2: Tb2XForm): Float; overload;
begin
case shape1.m_type of
e_circleShape:
case shape2.m_type of
e_circleShape: Result := b2Distance(x1, x2, Tb2CircleShape(shape1),
Tb2CircleShape(shape2), xf1, xf2);
e_polygonShape: Result := b2Distance(x2, x1, Tb2PolygonShape(shape2),
Tb2CircleShape(shape1), xf2, xf1);
else
Result := 0.0;
end;
e_polygonShape:
case shape2.m_type of
e_circleShape: Result := b2Distance(x1, x2, Tb2PolygonShape(shape1),
Tb2CircleShape(shape2), xf1, xf2);
e_polygonShape: Result := Distance_Generic_Simulate(x1, x2,
Tb2PolygonShape(shape1), shape2, xf1, xf2, False);
else
Result := 0.0;
end;
else
Result := 0.0;
end;
end;
{ b2PairManager.cpp }
// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
// This assumes proxyId1 and proxyId2 are 16-bit.
function PairManager_Hash(proxyId1, proxyId2: UInt32): UInt32;
begin
Result := (proxyId2 shl 16) or proxyId1;
Result := (not Result) + (Result shl 15);
Result := Result xor (Result shr 12);
Result := Result + (Result shl 2);
Result := Result xor (Result shr 4);
Result := Result * 2057;
Result := Result xor (Result shr 16);
end;
function PairManager_Equals(const pair: Tb2Pair; proxyId1,
proxyId2: Int32): Boolean; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
Result := (pair.proxyId1 = proxyId1) and (pair.proxyId2 = proxyId2);
end;
function PairManager_Equals(const pair1, pair2: Tb2BufferedPair): Boolean; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
begin
Result := (pair1.proxyId1 = pair2.proxyId1) and (pair1.proxyId2 = pair2.proxyId2);
end;
{ Tb2Segment }
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.4.1
// x = mu1 * p1 + mu2 * p2
// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
// mu1 = 1 - mu2;
// x = (1 - mu2) * p1 + mu2 * p2
// = p1 + mu2 * (p2 - p1)
// x = s + a * r (s := start, r := end - start)
// s + a * r = p1 + mu2 * d (d := p2 - p1)
// -a * r + mu2 * d = b (b := s - p1)
// [-r d] * [a; mu2] = b
// Cramer's rule:
// denom = det[-r d]
// a = det[b d] / denom
// mu2 = det[-r b] / denom
{$IFDEF OP_OVERLOAD}
function Tb2Segment.TestSegment(var lambda: Float; var normal: TVector2;
const segment: Tb2Segment; maxLambda: Float): Boolean;
const
k_slop = 100.0 * FLT_EPSILON;
var
s, r, d, n, b: TVector2;
denom, a, mu2: Float;
begin
s := segment.p1;
r := segment.p2 - s;
d := p2 - p1;
n := b2Cross(d, 1.0);
denom := -b2Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if denom > k_slop then
begin
// Does the segment intersect the infinite line associated with this segment?
b := s - p1;
a := b2Dot(b, n);
if (0.0 <= a) and (a <= maxLambda * denom) then
begin
mu2 := r.y * b.x - r.x * b.y;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2) and (mu2 <= denom * (1.0 + k_slop)) then
begin
n.Normalize;
lambda := a / denom;
normal := n;
Result := True;
Exit;
end;
end;
end;
Result := False;
end;
{$ENDIF}
{ Tb2AABB }
{$IFDEF OP_OVERLOAD}
function Tb2AABB.IsValid: Boolean;
var
d: TVector2;
begin
d := upperBound - lowerBound;
Result := (d.x >= 0.0) and (d.y >= 0.0) and upperBound.IsValid and lowerBound.IsValid;
end;
{$ENDIF}
{ Tb2DebugDraw }
constructor Tb2DebugDraw.Create;
begin
m_drawFlags := [];
m_shapeColor_Static := MakeColor(0.5, 0.9, 0.5);
m_shapeColor_Sleeping := MakeColor(0.5, 0.5, 0.9);
m_shapeColor_Normal := MakeColor(0.9, 0.9, 0.9);
m_pairColor := MakeColor(0.9, 0.9, 0.3);
m_aabbColor := MakeColor(0.9, 0.3, 0.9);
m_obbColor := MakeColor(0.5, 0.3, 0.5);
m_world_aabbColor := MakeColor(0.3, 0.9, 0.9);
m_coreColor := MakeColor(0.9, 0.6, 0.6);
m_jointLineColor := MakeColor(0.5, 0.8, 0.8);
end;
//////////////////////////////////////////////////////////////
// World
constructor Tb2World.Create(const worldAABB: Tb2AABB;
const gravity: TVector2; doSleep: Boolean);
var
bd: Tb2BodyDef;
begin
m_destructionListener := nil;
m_boundaryListener := nil;
m_contactFilter := b2_defaultFilter;
m_contactListener := nil;
m_debugDraw := nil;
m_bodyList := nil;
m_contactList := nil;
m_jointList := nil;
m_bodyCount := 0;
m_contactCount := 0;
m_jointCount := 0;
m_positionCorrection := True;
m_warmStarting := True;
m_continuousPhysics := True;
m_allowSleep := doSleep;
m_gravity := gravity;
m_lock := False;
m_inv_dt0 := 0.0;
m_contactManager := Tb2ContactManager.Create(Self);
m_broadPhase := Tb2BroadPhase.Create(worldAABB, m_contactManager);
bd := Tb2BodyDef.Create;
m_groundBody := CreateBody(bd);
end;
destructor Tb2World.Destroy;
var
p: Tb2Body;
begin
// Free all shapes
while Assigned(m_bodyList) do
begin
p := m_bodyList.m_next;
DestroyBody(m_bodyList);
m_bodyList := p;
end;
m_contactManager.Free;
m_broadPhase.Free;
end;
procedure Tb2World.Solve(const step: Tb2TimeStep);
var
i: Integer;
island: Tb2Island;
b, seed, other: Tb2Body;
c: Tb2Contact;
j: Tb2Joint;
stackCount: Int32;
stack: TList;
cn: Pb2ContactEdge;
jn: Pb2JointEdge;
begin
m_positionIterationCount := 0;
// Size the island for the worst case.
island := Tb2Island.Create(m_bodyCount, m_contactCount, m_jointCount, m_contactListener);
// Clear all the island flags.
b := m_bodyList;
while Assigned(b) do
begin
b.m_flags := b.m_flags and (not e_body_islandFlag);
b := b.m_next;
end;
c := m_contactList;
while Assigned(c) do
begin
c.m_flags := c.m_flags and (not e_contact_islandFlag);
c := c.m_next;
end;
j := m_jointList;
while Assigned(j) do
begin
j.m_islandFlag := False;
j := j.m_next;
end;
// Build and simulate all awake islands.
stack := TList.Create;
stack.Count := m_bodyCount;
seed := m_bodyList;
while Assigned(seed) do
begin
if (seed.m_flags and (e_body_islandFlag or e_sleepFlag or e_frozenFlag)) <> 0 then
begin
seed := seed.m_next;
Continue;
end;
if seed.IsStatic() then
begin
seed := seed.m_next;
Continue;
end;
// Reset island and stack.
island.Clear;
stackCount := 0;
stack[stackCount] := seed;
Inc(stackCount);
seed.m_flags := seed.m_flags or e_body_islandFlag;
// Perform a depth first search (DFS) on the constraint graph.
while (stackCount > 0) do
begin
// Grab the next body off the stack and add it to the island.
Dec(stackCount);
b := Tb2Body(stack[stackCount]);
island.Add(b);
// Make sure the body is awake.
b.m_flags := b.m_flags and (not e_sleepFlag);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if b.IsStatic() then
Continue;
// Search all contacts connected to this body.
cn := b.m_contactList;
while Assigned(cn) do
begin
// Has this contact already been added to an island?
if (cn^.contact.m_flags and (e_contact_islandFlag or e_nonSolidFlag)) <> 0 then
begin
cn := cn^.next;
Continue;
end;
// Is this contact touching?
if cn.contact.GetManifoldCount = 0 then
begin
cn := cn^.next;
Continue;
end;
island.Add(cn.contact);
cn.contact.m_flags := cn.contact.m_flags or e_contact_islandFlag;
other := cn.other;
// Was the other body already added to this island?
if (other.m_flags and e_body_islandFlag) <> 0 then
begin
cn := cn^.next;
Continue;
end;
//b2Assert(stackCount < stackSize);
stack[stackCount] := other;
Inc(stackCount);
other.m_flags := other.m_flags or e_body_islandFlag;
cn := cn^.next;
end;
// Search all joints connect to this body.
jn := b.m_jointList;
while Assigned(jn) do
begin
if jn^.joint.m_islandFlag then
begin
jn := jn^.next;
Continue;
end;
island.Add(jn^.joint);
jn^.joint.m_islandFlag := True;
other := jn^.other;
if (other.m_flags and e_body_islandFlag) <> 0 then
begin
jn := jn^.next;
Continue;
end;
//b2Assert(stackCount < stackSize);
stack[stackCount] := other;
Inc(stackCount);
other.m_flags := other.m_flags or e_body_islandFlag;
jn := jn^.next;
end;
end;
island.Solve(step, m_gravity, m_positionCorrection, m_allowSleep);
m_positionIterationCount := b2Max(m_positionIterationCount, island.m_positionIterationCount);
// Post solve cleanup.
for i := 0 to island.m_bodyCount - 1 do
begin
// Allow static bodies to participate in other islands.
b := Tb2Body(island.m_bodies[i]);
if b.IsStatic() then
b.m_flags := b.m_flags and (not e_body_islandFlag);
end;
seed := seed.m_next;
end;
stack.Free;
// Synchronize shapes, check for out of range bodies.
b := m_bodyList;
while Assigned(b) do
begin
if (b.m_flags and (e_sleepFlag or e_frozenFlag)) <> 0 then
begin
b := b.GetNext;
Continue;
end;
if b.IsStatic() then
begin
b := b.GetNext;
Continue;
end;
// Update shapes (for broad-phase). If the shapes go out of
// the world AABB then shapes and contacts may be destroyed,
// including contacts that are
// Did the body's shapes leave the world?
if (not b.SynchronizeShapes()) and Assigned(m_boundaryListener) then
m_boundaryListener.Violation(b);
b := b.GetNext;
end;
// Commit shape proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_broadPhase.Commit;
island.Free;
end;
procedure Tb2World.SolveTOI(const step: Tb2TimeStep);
var
i: Integer;
island: Tb2Island;
stackCount: Int32;
stack: TList;
b, b1, b2, seed, other: Tb2Body;
c: Tb2Contact;
minContact: Tb2Contact;
cn: Pb2ContactEdge;
minTOI, toi, t0: Float;
subStep: Tb2TimeStep;
inRange: Boolean;
begin
// Reserve an island and a stack for TOI island solution.
island := Tb2Island.Create(m_bodyCount, b2_maxTOIContactsPerIsland, 0, m_contactListener);
stack := TList.Create;
stack.Count := m_bodyCount;
b := m_bodyList;
while Assigned(b) do
begin
b.m_flags := b.m_flags and (not e_body_islandFlag);
b.m_sweep.t0 := 0.0;
b := b.m_next;
end;
c := m_contactList;
while Assigned(c) do
begin
// Invalidate TOI
c.m_flags := c.m_flags and (not (e_toiFlag or e_contact_islandFlag));
c := c.m_next;
end;
// Find TOI events and solve them.
while True do
begin
// Find the first TOI.
minContact := nil;
minTOI := 1.0;
c := m_contactList;
while Assigned(c) do
begin
if (c.m_flags and (e_slowFlag or e_nonSolidFlag)) <> 0 then
begin
c := c.m_next;
Continue;
end;
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
toi := 1.0;
if (c.m_flags and e_toiFlag) <> 0 then
toi := c.m_toi // This contact has a valid cached TOI.
else
begin
// Compute the TOI for this contact.
b1 := c.GetShape1.GetBody;
b2 := c.GetShape2.GetBody;
if (b1.IsStatic() or b1.IsSleeping()) and (b2.IsStatic() or b2.IsSleeping()) then
begin
c := c.m_next;
Continue;
end;
// Put the sweeps onto the same time interval.
t0 := b1.m_sweep.t0;
if b1.m_sweep.t0 < b2.m_sweep.t0 then
begin
t0 := b2.m_sweep.t0;
{$IFDEF OP_OVERLOAD}
b1.m_sweep.Advance(t0);
{$ELSE}
Advance(b1.m_sweep, t0);
{$ENDIF}
end
else if b2.m_sweep.t0 < b1.m_sweep.t0 then
begin
t0 := b1.m_sweep.t0;
{$IFDEF OP_OVERLOAD}
b2.m_sweep.Advance(t0);
{$ELSE}
Advance(b2.m_sweep, t0);
{$ENDIF}
end;
//b2Assert(t0 < 1.0);
// Compute the time of impact.
toi := b2TimeOfImpact(c.m_shape1, c.m_shape2, b1.m_sweep, b2.m_sweep);
//b2Assert(0.0 <= toi && toi <= 1.0);
if (toi > 0.0) and (toi < 1.0) then
toi := b2Min((1.0 - toi) * t0 + toi, 1.0);
c.m_toi := toi;
c.m_flags := c.m_flags or e_toiFlag;
end;
if (FLT_EPSILON < toi) and (toi < minTOI) then
begin
// This is the minimum TOI found so far.
minContact := c;
minTOI := toi;
end;
c := c.m_next;
end;
if (not Assigned(minContact)) or (1.0 - 100.0 * FLT_EPSILON < minTOI) then // No more TOI events. Done!
Break;
// Advance the bodies to the TOI.
b1 := minContact.GetShape1.GetBody;
b2 := minContact.GetShape2.GetBody;
b1.Advance(minTOI);
b2.Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactListener);
minContact.m_flags := minContact.m_flags and (not e_toiFlag);
if minContact.GetManifoldCount = 0 then // This shouldn't happen. Numerical error?
Continue;
// Build the TOI island. We need a dynamic seed.
seed := b1;
if seed.IsStatic() then
seed := b2;
// Reset island and stack.
island.Clear;
stackCount := 0;
stack[stackCount] := seed;
Inc(stackCount);
seed.m_flags := seed.m_flags or e_body_islandFlag;
// Perform a depth first search (DFS) on the contact graph.
while (stackCount > 0) do
begin
// Grab the next body off the stack and add it to the island.
Dec(stackCount);
b := Tb2Body(stack[stackCount]);
island.Add(b);
// Make sure the body is awake.
b.m_flags := b.m_flags and (not e_sleepFlag);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if b.IsStatic() then
Continue;
// Search all contacts connected to this body.
cn := b.m_contactList;
while Assigned(cn) do
begin
// Does the TOI island still have space for contacts?
if island.m_contactCount = island.m_contactCapacity then
begin
cn := cn.next;
Continue;
end;
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if (cn.contact.m_flags and (e_contact_islandFlag or
e_slowFlag or e_nonSolidFlag)) <> 0 then
begin
cn := cn.next;
Continue;
end;
// Is this contact touching? For performance we are not updating this contact.
if cn.contact.GetManifoldCount = 0 then
begin
cn := cn.next;
Continue;
end;
island.Add(cn.contact);
cn.contact.m_flags := cn.contact.m_flags or e_contact_islandFlag;
// Update other body.
other := cn.other;
// Was the other body already added to this island?
if (other.m_flags and e_body_islandFlag) <> 0 then
begin
cn := cn.next;
Continue;
end;
// March forward, this can do no harm since this is the min TOI.
if not other.IsStatic() then
begin
other.Advance(minTOI);
other.WakeUp;
end;
//b2Assert(stackCount < stackSize);
stack[stackCount] := other;
Inc(stackCount);
other.m_flags := other.m_flags or e_body_islandFlag;
cn := cn.next;
end;
end;
subStep.dt := (1.0 - minTOI) * step.dt;
//b2Assert(subStep.dt > B2_FLT_EPSILON);
subStep.inv_dt := 1.0 / subStep.dt;
subStep.maxIterations := step.maxIterations;
island.SolveTOI(subStep);
// Post solve cleanup.
for i := 0 to island.m_bodyCount - 1 do
begin
// Allow bodies to participate in future TOI islands.
b := Tb2Body(island.m_bodies[i]);
b.m_flags := b.m_flags and (not e_body_islandFlag);
if (b.m_flags and (e_sleepFlag or e_frozenFlag)) <> 0 then
Continue;
if b.IsStatic() then
Continue;
// Update shapes (for broad-phase). If the shapes go out of
// the world AABB then shapes and contacts may be destroyed,
// including contacts that are
inRange := b.SynchronizeShapes;
// Did the body's shapes leave the world?
if (not inRange) and Assigned(m_boundaryListener) then
m_boundaryListener.Violation(b);
// Invalidate all contact TOIs associated with this body. Some of these
// may not be in the island because they were not touching.
cn := b.m_contactList;
while Assigned(cn) do
begin
cn.contact.m_flags := cn.contact.m_flags and (not e_toiFlag);
cn := cn.next;
end;
end;
for i := 0 to island.m_contactCount - 1 do
with Tb2Contact(island.m_contacts[i]) do
begin
// Allow contacts to participate in future TOI islands.
m_flags := m_flags and (not (e_toiFlag or e_contact_islandFlag));
end;
// Commit shape proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_broadPhase.Commit;
end;
stack.Free;
island.Free;
end;
procedure Tb2World.DrawDebugData;
var
core: Boolean;
xf: Tb2XForm;
b: Tb2Body;
s: Tb2Shape;
j: Tb2Joint;
invQ, h: TVector2;
i: Integer;
b1, b2: Tb2AABB;
index: UInt16;
vs: TVectorArray4;
begin
if not Assigned(m_debugDraw) then
Exit;
with m_debugDraw do
begin
if e_shapeBit in m_drawFlags then
begin
core := e_coreShapeBit in m_drawFlags;
b := m_bodyList;
while Assigned(b) do
begin
xf := b.m_xf;
s := b.GetShapeList;
while Assigned(s) do
begin
if b.IsStatic then
DrawShape(s, xf, m_shapeColor_Static, core)
else if b.IsSleeping then
DrawShape(s, xf, m_shapeColor_Sleeping, core)
else
DrawShape(s, xf, m_shapeColor_Normal, core);
s := s.m_next;
end;
b := b.GetNext;
end;
end;
if e_jointBit in m_drawFlags then
begin
j := m_jointList;
while Assigned(j) do
begin
if j.m_type <> e_mouseJoint then
DrawJoint(j);
j := j.m_next;
end;
end;
with m_broadPhase do
begin
if e_pairBit in m_drawFlags then
begin
{$IFDEF OP_OVERLOAD}
invQ.SetValue(1.0 / m_quantizationFactor.x, 1.0 / m_quantizationFactor.y);
{$ELSE}
SetValue(invQ, 1.0 / m_quantizationFactor.x, 1.0 / m_quantizationFactor.y);
{$ENDIF}
for i := 0 to b2_tableCapacity - 1 do
begin
index := m_pairManager.m_hashTable[i];
while (index <> b2_nullPair) do
with m_pairManager.m_pairs[index] do
begin
with m_proxyPool[proxyId1] do
begin
b1.lowerBound.x := m_worldAABB.lowerBound.x + invQ.x * m_bounds[0][lowerBounds[0]].value;
b1.lowerBound.y := m_worldAABB.lowerBound.y + invQ.y * m_bounds[1][lowerBounds[1]].value;
b1.upperBound.x := m_worldAABB.lowerBound.x + invQ.x * m_bounds[0][upperBounds[0]].value;
b1.upperBound.y := m_worldAABB.lowerBound.y + invQ.y * m_bounds[1][upperBounds[1]].value;
end;
with m_proxyPool[proxyId2] do
begin
b2.lowerBound.x := m_worldAABB.lowerBound.x + invQ.x * m_bounds[0][lowerBounds[0]].value;
b2.lowerBound.y := m_worldAABB.lowerBound.y + invQ.y * m_bounds[1][lowerBounds[1]].value;
b2.upperBound.x := m_worldAABB.lowerBound.x + invQ.x * m_bounds[0][upperBounds[0]].value;
b2.upperBound.y := m_worldAABB.lowerBound.y + invQ.y * m_bounds[1][upperBounds[1]].value;
end;
{$IFDEF OP_OVERLOAD}
DrawSegment(0.5 * (b1.lowerBound + b1.upperBound),
0.5 * (b2.lowerBound + b2.upperBound), m_pairColor);
{$ELSE}
DrawSegment(Multiply(Add(b1.lowerBound, b1.upperBound), 0.5),
Multiply(Add(b2.lowerBound, b2.upperBound), 0.5), m_pairColor);
{$ENDIF}
index := next;
end;
end;
end;
if e_aabbBit in m_drawFlags then
begin
{$IFDEF OP_OVERLOAD}
invQ.SetValue(1.0 / m_quantizationFactor.x, 1.0 / m_quantizationFactor.y);
{$ELSE}
SetValue(invQ, 1.0 / m_quantizationFactor.x, 1.0 / m_quantizationFactor.y);
{$ENDIF}
for i := 0 to b2_maxProxies - 1 do
begin
with m_proxyPool[i] do
begin
{$IFDEF OP_OVERLOAD}
if not IsValid then
{$ELSE}
if not IsValid(m_proxyPool[i]) then
{$ENDIF}
Continue;
b1.lowerBound.x := m_worldAABB.lowerBound.x + invQ.x * m_bounds[0][lowerBounds[0]].value;
b1.lowerBound.y := m_worldAABB.lowerBound.y + invQ.y * m_bounds[1][lowerBounds[1]].value;
b1.upperBound.x := m_worldAABB.lowerBound.x + invQ.x * m_bounds[0][upperBounds[0]].value;
b1.upperBound.y := m_worldAABB.lowerBound.y + invQ.y * m_bounds[1][upperBounds[1]].value;
end;
vs[0] := b1.lowerBound;
vs[2] := b1.upperBound;
{$IFDEF OP_OVERLOAD}
vs[1].SetValue(b1.upperBound.x, b1.lowerBound.y);
vs[3].SetValue(b1.lowerBound.x, b1.upperBound.y);
{$ELSE}
SetValue(vs[1], b1.upperBound.x, b1.lowerBound.y);
SetValue(vs[3], b1.lowerBound.x, b1.upperBound.y);
{$ENDIF}
DrawPolygon4(vs, 4, m_aabbColor);
end;
vs[0] := m_worldAABB.lowerBound;
vs[2] := m_worldAABB.upperBound;
{$IFDEF OP_OVERLOAD}
vs[1].SetValue(m_worldAABB.upperBound.x, m_worldAABB.lowerBound.y);
vs[3].SetValue(m_worldAABB.lowerBound.x, m_worldAABB.upperBound.y);
{$ELSE}
SetValue(vs[1], m_worldAABB.upperBound.x, m_worldAABB.lowerBound.y);
SetValue(vs[3], m_worldAABB.lowerBound.x, m_worldAABB.upperBound.y);
{$ENDIF}
DrawPolygon4(vs, 4, m_world_aabbColor);
end;
end;
if e_obbBit in m_drawFlags then
begin
b := m_bodyList;
while Assigned(b) do
begin
xf := b.m_xf;
s := b.GetShapeList;
while Assigned(s) do
begin
if s.GetType <> e_polygonShape then
begin
s := s.m_next;
Continue;
end;
h := Tb2PolygonShape(s).m_obb.extents;
{$IFDEF OP_OVERLOAD}
vs[0].SetValue(-h.x, -h.y);
vs[1].SetValue(h.x, -h.y);
vs[3].SetValue(-h.x, h.y);
{$ELSE}
SetValue(vs[0], -h.x, -h.y);
SetValue(vs[1], h.x, -h.y);
SetValue(vs[3], -h.x, h.y);
{$ENDIF}
vs[2] := h;
for i := 0 to 3 do
with Tb2PolygonShape(s).m_obb do
begin
{$IFDEF OP_OVERLOAD}
vs[i] := center + b2Mul(R, vs[i]);
{$ELSE}
vs[i] := Add(center, b2Mul(R, vs[i]));
{$ENDIF}
vs[i] := b2Mul(xf, vs[i]);
end;
DrawPolygon4(vs, 4, m_obbColor);
s := s.m_next;
end;
b := b.GetNext;
end;
end;
if e_centerOfMassBit in m_drawFlags then
begin
b := m_bodyList;
while Assigned(b) do
begin
xf := b.m_xf;
xf.position := b.GetWorldCenter;
DrawXForm(xf);
b := b.GetNext;
end;
end;
end;
end;
procedure Tb2World.DrawShape(shape: Tb2Shape; const xf: Tb2XForm;
const color: RGBA; core: Boolean);
var
i: Integer;
center: TVector2;
vertices: Tb2PolyVertices;
begin
with m_debugDraw do
case shape.GetType of
e_circleShape:
with Tb2CircleShape(shape) do
begin
center := b2Mul(xf, m_localPosition);
DrawSolidCircle(center, xf.R.col1, GetRadius, color);
if core then
DrawCircle(center, GetRadius - b2_toiSlop, m_coreColor);
end;
e_polygonShape:
with Tb2PolygonShape(shape) do
begin
//b2Assert(vertexCount <= b2_maxPolygonVertices);
for i := 0 to GetVertexCount - 1 do
vertices[i] := b2Mul(xf, m_vertices[i]);
DrawSolidPolygon(vertices, GetVertexCount, color);
if core then
begin
for i := 0 to GetVertexCount - 1 do
vertices[i] := b2Mul(xf, m_coreVertices[i]);
DrawPolygon(vertices, GetVertexCount, m_coreColor);
end;
end;
end;
end;
procedure Tb2World.DrawJoint(joint: Tb2Joint);
var
p1, p2, s1, s2: TVector2;
begin
p1 := joint.GetAnchor1;
p2 := joint.GetAnchor2;
with m_debugDraw do
begin
case joint.GetType of
e_distanceJoint: DrawSegment(p1, p2, m_jointLineColor);
e_pulleyJoint:
with Tb2PulleyJoint(joint) do
begin
s1 := GetGroundAnchor1;
s2 := GetGroundAnchor2;
DrawSegment(s1, p1, m_jointLineColor);
DrawSegment(s2, p2, m_jointLineColor);
DrawSegment(s1, s2, m_jointLineColor);
end;
e_mouseJoint: ;
e_fixedJoint: DrawSegment(p1, p2, m_jointLineColor);
else
DrawSegment(joint.GetBody1.m_xf.position, p1, m_jointLineColor);
DrawSegment(p1, p2, m_jointLineColor);
DrawSegment(joint.GetBody2.m_xf.position, p2, m_jointLineColor);
end;
end;
end;
function Tb2World.CreateBody(def: Tb2BodyDef; AutoFreeBodyDef: Boolean = True): Tb2Body;
begin
//b2Assert(m_lock == False);
if m_lock then
begin
Result := nil;
Exit;
end;
Result := Tb2Body.Create(def, Self);
// Add to world doubly linked list.
Result.m_prev := nil;
Result.m_next := m_bodyList;
if Assigned(m_bodyList) then
m_bodyList.m_prev := Result;
m_bodyList := Result;
Inc(m_bodyCount);
if AutoFreeBodyDef then
def.Free;
end;
procedure Tb2World.DestroyBody(body: Tb2Body; DoFree: Boolean = True);
var
jn, jn0: Pb2JointEdge;
s, s0: Tb2Shape;
begin
//b2Assert(m_bodyCount > 0);
//b2Assert(m_lock == False);
if m_lock then
Exit;
// Delete the attached joints.
jn := body.m_jointList;
if Assigned(m_destructionListener) then
begin
while Assigned(jn) do
begin
jn0 := jn;
jn := jn^.next;
m_destructionListener.SayGoodbye(jn0^.joint);
DestroyJoint(jn0^.joint);
end;
end
else
begin
while Assigned(jn) do
begin
jn0 := jn;
jn := jn^.next;
DestroyJoint(jn0^.joint);
end;
end;
// Delete the attached shapes. This destroys broad-phase
// proxies and pairs, leading to the destruction of contacts.
s := body.m_shapeList;
if Assigned(m_destructionListener) then
begin
while Assigned(s) do
begin
s0 := s;
s := s.m_next;
m_destructionListener.SayGoodbye(s0);
s0.DestroyProxy(m_broadPhase);
s0.Free;
end;
end
else
begin
while Assigned(s) do
begin
s0 := s;
s := s.m_next;
s0.DestroyProxy(m_broadPhase);
s0.Free;
end;
end;
// Remove world body list.
if Assigned(body.m_prev) then
body.m_prev.m_next := body.m_next;
if Assigned(body.m_next) then
body.m_next.m_prev := body.m_prev;
if body = m_bodyList then
m_bodyList := body.m_next;
Dec(m_bodyCount);
if DoFree then
body.Destroy2;
end;
function Tb2World.CreateJoint(def: Tb2JointDef; AutoFreeJointDef: Boolean = True): Tb2Joint;
var
j: Tb2Joint;
b: Tb2Body;
s: Tb2Shape;
begin
//b2Assert(m_lock == False);
Result := nil;
case def.JointType of
e_unknownJoint: Exit;
e_revoluteJoint: j := Tb2RevoluteJoint.Create(Tb2RevoluteJointDef(def));
e_prismaticJoint: j := Tb2PrismaticJoint.Create(Tb2PrismaticJointDef(def));
e_distanceJoint: j := Tb2DistanceJoint.Create(Tb2DistanceJointDef(def));
e_pulleyJoint: j := Tb2PulleyJoint.Create(Tb2PulleyJointDef(def));
e_mouseJoint: j := Tb2MouseJoint.Create(Tb2MouseJointDef(def));
e_gearJoint: j := Tb2GearJoint.Create(Tb2GearJointDef(def));
e_fixedJoint: j := Tb2FixedJoint.Create(Tb2FixedJointDef(def));
end;
// Connect to the world list.
j.m_prev := nil;
j.m_next := m_jointList;
if Assigned(m_jointList) then
m_jointList.m_prev := j;
m_jointList := j;
Inc(m_jointCount);
// Connect to the bodies' doubly linked lists.
j.m_node1.joint := j;
j.m_node1.other := j.m_body2;
j.m_node1.prev := nil;
j.m_node1.next := j.m_body1.m_jointList;
if Assigned(j.m_body1.m_jointList) then
j.m_body1.m_jointList.prev := @j.m_node1;
j.m_body1.m_jointList := @j.m_node1;
j.m_node2.joint := j;
j.m_node2.other := j.m_body1;
j.m_node2.prev := nil;
j.m_node2.next := j.m_body2.m_jointList;
if Assigned(j.m_body2.m_jointList) then
j.m_body2.m_jointList.prev := @j.m_node2;
j.m_body2.m_jointList := @j.m_node2;
// If the joint prevents collisions, then reset collision filtering.
if not def.collideConnected then
begin
// Reset the proxies on the body with the minimum number of shapes.
if def.body1.m_shapeCount < def.body2.m_shapeCount then
b := def.body1
else