//Convex Shape Dynamics v2.0
//Author: Dan
unit CSD;

interface

uses
  Classes, Types;

type
  TCSWorld = class;
  TCSScene = class;
  TCSSceneTree = class;
  TCSSceneObject = class;
  TCSColy = class;
  TCSDyna = class;
  TCSPrim = class;
  TCSPrimCircle = class;
  TCSPrimPolygon = class;
  TCSShape = class;
  TCSBody = class;
  TCSChain = class;

  PCSMatrix2 = ^TCSMatrix2;
  TCSMatrix2 = array[0..1, 0..1] of single;

  PCSVector = ^TCSVector;
  TCSVector = record
  public
    x, y: single;
    class operator Negative(const v1: TCSVector): TCSVector;
    class operator Add(const v1, v2: TCSVector): TCSVector;
    class operator Add(const v1: TCSVector; const s: single): TCSVector;
    class operator Add(const s: single; const v1: TCSVector): TCSVector;
    class operator Subtract(const v1, v2: TCSVector): TCSVector;
    class operator Subtract(const v1: TCSVector; const s: single): TCSVector;
    class operator Multiply(const v1, v2: TCSVector): single;
    class operator Multiply(const v1: TCSVector; const s: single): TCSVector;
    class operator Multiply(const v1: TCSVector; const m: TCSMatrix2): TCSVector;
    class operator Divide(const v1: TCSVector; const s: single): TCSVector;
  end;

  PCSVectorArray = ^TCSVectorArray;
  TCSVectorArray = array[0..0] of TCSVector;

  PCSPolygon = ^TCSPolygon;
  TCSPolygon = array of TCSVector;

  PCSCircle = ^TCSCircle;
  TCSCircle = record
    Pos: TCSVector;
    Radius: single;
  end;

  PCSResponse = ^TCSResponse;
  TCSResponse = record
    N: TCSVector;
    D: single;
  end;

  PCSResponseArray = ^TCSResponseArray;
  TCSResponseArray = array[0..0] of TCSResponse;

  PCSContact = ^TCSContact;
  TCSContact = record
    Pos: TCSVector;
    N1, N2: TCSVector;
    Mag: single;
  end;
  TCSContactArray = array of TCSContact;

  PCSContactGroup = ^TCSContactGroup;
  TCSContactGroup = record
    Body1, Body2: TCSBody;
    Contacts: TCSChain;
    R: TCSResponse;
  end;

  PCSLink = ^TCSLink;
  TCSLink = record
    Data: Pointer;
    Prev, Next: PCSLink;
  end;
  
  TCSChain = class
  private
    m_First: PCSLink;
    m_Last: PCSLink;
    m_Current: PCSLink;
    m_Count: DWORD;
  protected
    function GetCurrent: Pointer;
    function GetFirst: Pointer;
  public
    constructor Create;
    destructor Destroy; override;
    property Current: Pointer read GetCurrent;
    property First: Pointer read GetFirst;
    property Count: DWORD read m_Count;
    function Next: Pointer;
    function AddData(const Data: Pointer): PCSLink;
    function AddLink(const Link: PCSLink): PCSLink;
    procedure RemoveData(const Data: Pointer);
    procedure RemoveLink(const Link: PCSLink);
    procedure RemoveCurrent;
    procedure Reset;
    procedure Clear;
  end;

  TCSPreColyCallback = procedure (const Body1, Body2: TCSBody; var Proceed: boolean);
  TCSColyCallback = procedure (const Body1, Body2: TCSBody; const Contacts: TCSChain; const R: TCSResponse);
  TCSProcCallback = procedure (const Body: TCSBody);

  TCSWorld = class
  private
    m_Scene: TCSScene;
    m_Bodies: TCSChain;
    m_BodiesDisabled: TCSChain;
    m_Coly: TCSColy;
    m_Dyna: TCSDyna;
    m_ContactGroups: TList;
    m_ColyCallback: TCSColyCallback;
    m_PreColyCallback: TCSPreColyCallback;
    m_ProcCallback: TCSProcCallback;
    m_Initialized: boolean;
  protected
    function AddContactGroup(
      const Body1, Body2: TCSBody;
      const Contacts: TCSchain;
      const R: TCSResponse
    ): PCSContactGroup;
    procedure Collide(Body1, Body2: TCSBody);
  public
    constructor Create;
    destructor Destroy; override;
    property Coly: TCSColy read m_Coly;
    property Dyna: TCSDyna read m_Dyna;
    property ColyCallback: TCSColyCallback read m_ColyCallback write m_ColyCallback;
    property PreColyCallback: TCSPreColyCallback read m_PreColyCallback write m_PreColyCallback;
    procedure InitWorld(
      const X, Y, SizeX, SizeY: single;
      const Depth: WORD = 4
    );
    procedure UnInit;
    function CreateBody: TCSBody;
    procedure DestroyBody(const Body: TCSBody);
    procedure FreeBodies;
    procedure FreeContactGroups;
    procedure Step(const StepSize: single);
  end;

  TCSSceneRect = record
  case Integer of
    0: (X1, Y1, X2, Y2: single);
    1: (Min, Max: TCSVector);
  end;

  TCSScene = class
  private
    m_Depth: WORD;
    m_ParentTree: TCSSceneTree;
    m_World: TCSWorld;
    m_QueryDown: boolean;
  public
    constructor Create;
    destructor Destroy; override;
    procedure Init(
      const X, Y, SizeX, SizeY: single;
      const Depth: WORD
    );
    procedure UnInit;
    procedure AddObject(Obj: TCSSceneObject);
    procedure QueryCollision(Body: TCSBody);
  end;

  TCSSceneTReeType = (
    ttRandom,
    ttQuadDiv,
    ttYDiv,
    ttXDiv
  );

  TCSSceneTree = class
  private
    m_TreeType: TCSSceneTReeType;
    m_Parent: TCSSceneTree;
    m_SubTrees: array of TCSSceneTree;
    m_UpLinks: array of TCSSceneTree;
    m_Objects: TList;
    m_Bounds: TCSSceneRect;
    m_Patch: boolean;
  protected
    procedure CreateSubTrees(const Count: integer);
    function MakeSubTree: TCSSceneTree;
    function TryFit(Obj: TCSSceneObject): boolean;
    function AddTree(Tree: TCSSceneTree): boolean;
    procedure PatchTree;
    procedure DetectBounds;
  public
    constructor Create;
    destructor Destroy; override;
    property Bounds: TCSSceneRect read m_Bounds;
    procedure FreeObjects;
    procedure FreeSubTrees;
    procedure Init;
    procedure AddObject(Obj: TCSSceneObject);
  end;
  
  TCSSceneObject = class
  private
    m_Pos: TCSVector;
    m_Radius: single;
    m_Tree: TCSSceneTree;
    m_Body: TCSBody;
  protected
    procedure ReAllocate;
    procedure SetPos(Value: TCSVector);
    procedure SetRadius(Value: single);
  public
    constructor Create;
    destructor Destroy; override;
    property Tree: TCSSceneTree read m_Tree;
    property Body: TCSBody read m_Body;
    property Pos: TCSVector read m_Pos write SetPos;
    property Radius: single read m_Radius write SetRadius;
    procedure Remove;
  end;

  TCSColy = class
  private
  public
    constructor Create;
    destructor Destroy; override;
    function CollideShapes(
      const S1, S2: TCSShape;
      const Contacts: TCSChain;
      var R: TCSResponse
    ): boolean;
    function CollidePrims(
      const P1, P2: TCSPrim;
      var Contacts: TCSContactArray;
      var R: TCSResponse
    ): boolean;
  end;

  TCSDyna = class
  private
  public
    constructor Create;
    destructor Destroy; override;
    function GetForceAtPoint(
      const Body: TCSBody;
      const P: TCSVector
    ): TCSVector;
    procedure SetForceAtPoint(
      const Body: TCSBody;
      const P, F: TCSVector
    );
    procedure ResolveContacts(
      const Body1, Body2: TCSBody;
      const Contacts: TCSChain;
      const R: TCSResponse
    );
    function ResolveForces(
      const N1: TCSVector;
      const N2: TCSVector;
      const F1: TCSVector;
      const F2: TCSVector;
      var Direct1: TCSVector;
      var Friction1: TCSVector;
      var Direct2: TCSVector;
      var Friction2: TCSVector;
      const MagHardness: single = 1;
      const MagFriction: single = 1
    ): boolean;
  end;

  TCSPrimType = (
    ptPoly,
    ptCircle
  );

  TCSPrim = class
  private
    m_PrimType: TCSPrimType;
  protected
    function GetCenter: TCSVector; virtual; abstract;
  public
    constructor Create; virtual;
    destructor Destroy; override;
    property PrimType: TCSPrimType read m_PrimType;
    property Center: TCSVector read GetCenter;
    procedure TransformMove(const Pos: TCSVector); virtual; abstract;
    procedure TransformRotate(const m: TCSMatrix2); overload; virtual; abstract;
    procedure TransformRotate(const Angle: single); overload;
    function Duplicate: TCSPrim; virtual; abstract;
    function GetRadius: single; virtual; abstract;
  end;

  TCSPrimCircle = class (TCSPrim)
  private
    m_Circle: TCSCircle;
  protected
    function GetCenter: TCSVector; override;
    function GetCircle: PCSCircle;
  public
    constructor Create; override;
    property Circle: PCSCircle read GetCircle;
    procedure TransformMove(const Pos: TCSVector); override;
    procedure TransformRotate(const m: TCSMatrix2); override;
    function Duplicate: TCSPrim; override;
    function GetRadius: single; override;
  end;

  TCSPrimPolygon = class (TCSPrim)
  private
    m_Polygon: TCSPolygon;
  protected
    function GetCenter: TCSVector; override;
    function GetPolygon: PCSPolygon;
  public
    constructor Create; override;
    property Polygon: PCSPolygon read GetPolygon;
    procedure TransformMove(const Pos: TCSVector); override;
    procedure TransformRotate(const m: TCSMatrix2); override;
    function Duplicate: TCSPrim; override;
    function GetRadius: single; override;
  end;

  TCSShape = class
  private
    m_Prims: TCSChain;
    m_Radius: single;
  public
    constructor Create;
    destructor Destroy; override;
    property Prims: TCSChain read m_Prims;
    property Radius: single read m_Radius;
    procedure AddPrim(const Prim: TCSPrim);
    procedure AddCircle(const Pos: TCSVector; const Radius: single);
    procedure AddPolygon(const P: TCSPolygon);
    procedure Clear;
    procedure TransformMove(const Pos: TCSVector);
    procedure TransformRotate(const m: TCSMatrix2); overload;
    procedure TransformRotate(const Angle: single); overload;
    procedure AutoCM;
    function Duplicate: TCSShape;
  end;

  TCSBody = class
  private
    m_World: TCSWorld;
    m_ShapeInit: TCSShape;
    m_ShapeMod: TCSShape;
    m_ShapeSet: boolean;
    m_SceneObject: TCSSceneObject;
    m_Link: PCSLink;
    m_Rad: single;
    m_RadSq: single;
    m_RadSqInv: single;
    m_Pos: TCSVector;
    m_Rot: TCSMatrix2;
    m_VelLinear: TCSVector;
    m_VelAngular: single;
    m_AccLinear: TCSVector;
    m_AccAngular: single;
    m_ImpLinear: TCSVector;
    m_ImpAngular: single;
    m_Mass: single;
    m_MassInv: single;
    m_Inertia: single;
    m_InertiaInv: single;
    m_Hardness: single;
    m_Friction: single;
    m_IsStatic: boolean;
    m_IsEnabled: boolean;
    m_Data: Pointer;
  protected
    procedure FreeShapes;
    procedure SetShape(S: TCSShape);
    procedure Transform;
    function GetPos: PCSVector;
    function GetRot: PCSMatrix2;
    function GetVelLinear: PCSVector;
    function GetVelAngular: PSingle;
    function GetAccLinear: PCSVector;
    function GetAccAngular: PSingle;
    function GetImpLinear: PCSVector;
    function GetImpAngular: PSingle;
    function GetAng: single;
    procedure SetMass(Value: single);
    procedure SetInertia(Value: single);
    procedure SetEnabled(Value: boolean);
  public
    constructor Create;
    destructor Destroy; override;
    property SceneObject: TCSSceneObject read m_SceneObject;
    property Shape: TCSShape read m_ShapeMod write SetShape;
    property Pos: PCSVector read GetPos;
    property Rot: PCSMatrix2 read GetRot;
    property Ang: single read GetAng;
    property VelLinear: PCSVector read GetVelLinear;
    property VelAngular: PSingle read GetVelAngular;
    property AccLinear: PCSVector read GetAccLinear;
    property AccAngular: PSingle read GetAccAngular;
    property ImpLinear: PCSVector read GetImpLinear;
    property ImpAngular: PSingle read GetImpAngular;
    property Mass: single read m_Mass write SetMass;
    property Inertia: single read m_Inertia write SetInertia;
    property Hardness: single read m_Hardness write m_Hardness;
    property Friction: single read m_Friction write m_Friction;
    property IsStatic: boolean read m_IsStatic write m_IsStatic;
    property IsEnabled: boolean read m_IsEnabled write SetEnabled;
    property Data: Pointer read m_Data write m_Data;
    procedure Step(const StepSize: single);
  end;

//Vector Functions BEGIN
function CSVector(const x, y: single): TCSVector;
function CSVecLength(const v: TCSVector): single;
function CSVecAdd(const v1, v2: TCSVector): TCSVector;
function CSVecSubtract(const v1, v2: TCSVector): TCSVector;
function CSVecNormalize(const v: TCSVector): TCSVector;
function CSVecDot(const v1, v2: TCSVector): single;
function CSVecCross(const v1, v2: TCSVector): single;
function CSVecTransform(const v1: TCSVector; const m: TCSMatrix2): TCSVector;
function CSVecInPolygon(const v1: TCSVector; const P1: TCSPolygon): boolean;
function CSVecDistance(const v1, v2: TCSVector): single;
//Vector Functions END

//Matrix Functions BEGIN
function CSMatrixIdentity: TCSMatrix2;
function CSMatrixScaling(const ScaleX, ScaleY: single): TCSMatrix2;
function CSMatrixRotation(const Angle: single): TCSMAtrix2;
function CSMatrixAdd(const m1, m2: TCSMatrix2): TCSMatrix2;
function CSMatrixMultiply(const m1, m2: TCSMatrix2): TCSMatrix2;
function CSMatrixScale(const m: TCSMatrix2; s: single): TCSMatrix2;
function CSMatrixDeterminant(const m: TCSMatrix2): single;
function CSMatrixTranspose(const m: TCSMatrix2): TCSMatrix2;
function CSMatrixInverse(const m: TCSMatrix2): TCSMatrix2;
function CSMatrixExtractRotation(const m: TCSMatrix2): single;
//Matrix Functions END

//Polygon Functions BEGIN
procedure CSPolyEdge(
  const P: TCSPolygon;
  const EdgeIndex: integer;
  var Index1, Index2: integer
);
function CSPolyEdgeDir(
  const P: TCSPolygon;
  const EdgeIndex: integer
): TCSVector;
function CSPolyEdgeNormal(
  const P: TCSPolygon;
  const EdgeIndex: integer
): TCSVector;
function CSPolyCenter(
  const P: TCSPolygon
): TCSVector;
procedure CSPolyMove(
  var P: TCSPolygon;
  const Pos: TCSVector
);
procedure CSPolyTransform(
  var P: TCSPolygon;
  const m: TCSMatrix2
);
//Polygon Functions BEGIN

//Utility Functions BEGIN
procedure CSPolyProject(
  const Axis: TCSVector;
  const P: TCSPolygon;
  var Min, Max: single
);
procedure CSCircleProject(
  const Axis: TCSVector;
  const C: TCSCircle;
  var Min, Max: single
);
function CSPolyAxisSeparate(
  const Axis: TCSVector;
  const P1, P2: TCSPolygon;
  const R: PCSResponse = nil
): boolean;
function CSCircleAxisSeparate(
  const Axis: TCSVector;
  const C1, C2: TCSCircle;
  const R: PCSResponse = nil
): boolean;
function CSPolyVsCircleAxisSeparate(
  const Axis: TCSVector;
  const P: TCSPolygon;
  const C: TCSCircle;
  const R: PCSResponse = nil
): boolean;
function CSCircleVsPolyAxisSeparate(
  const Axis: TCSVector;
  const C: TCSCircle;
  const P: TCSPolygon;
  const R: PCSResponse = nil
): boolean;
function CSPolyIntersect(
  const P1, P2: TCSPolygon;
  const R: PCSResponse = nil
): boolean;
function CSCircleIntersect(
  const C1, C2: TCSCircle;
  const R: PCSResponse = nil
): boolean;
function CSPolyVsCircleIntersect(
  const P: TCSPolygon;
  const C: TCSCircle;
  const R: PCSResponse = nil
): boolean;
function CSCircleVsPolyIntersect(
  const C: TCSCircle;
  const P: TCSPolygon;
  const R: PCSResponse = nil
): boolean;
function CSPolyFindContacts(
  const N: TCSVector;
  const P1, P2: TCSPolygon
): TCSContactArray;
function CSCircleFindContacts(
  const N: TCSVector;
  const C1, C2: TCSCircle
): TCSContactArray;
function CSPolyVsCircleFindContacts(
  const N: TCSVector;
  const P: TCSPolygon;
  const C: TCSCircle
): TCSContactArray;
function CSCircleVsPolyFindContacts(
  const N: TCSVector;
  const C: TCSCircle;
  const P: TCSPolygon
): TCSContactArray;
function CSFindMTD(
  const RArr: PCSResponseArray;
  const RCount: integer
): TCSResponse;
procedure ClearContactChain(
  const Contacts: TCSChain
);
function CSSceneRect(
  const X1, Y1, X2, Y2: single
): TCSSceneRect; overload;
function CSSceneRect(
  const Min, Max: TCSVector
): TCSSceneRect; overload;
//Utility Functions END

implementation

//TCSChain BEGIN
constructor TCSChain.Create;
begin
  inherited Create;
  m_First := nil;
  m_Current := nil;
  m_Last := nil;
  m_Count := 0;
end;

destructor TCSChain.Destroy;
begin
  Clear;
  inherited Destroy;
end;

function TCSChain.GetCurrent: Pointer;
begin
  Result := m_Current^.Data;
end;

function TCSChain.GetFirst: Pointer;
begin
  Result := m_First^.Data;
end;

function TCSChain.Next: Pointer;
begin
  if m_Current = nil then m_Current := m_First
  else
  m_Current := m_Current.Next;
  if m_Current = nil then Result := nil
  else
  Result := m_Current.Data;
end;

function TCSChain.AddData(const Data: Pointer): PCSLink;
var
  Link: PCSLink;
begin
  New(Link);
  Link.Data := Data;
  Link.Prev := nil;
  Link.Next := nil;
  Result := AddLink(Link);
end;

function TCSChain.AddLink(const Link: PCSLink): PCSLink;
begin
  if m_Count = 0 then
  m_First := Link
  else
  begin
    m_Last^.Next := Link;
    Link^.Prev := m_Last;
  end;
  m_Last := Link;
  m_Count := m_Count + 1;
  Result := Link;
end;

procedure TCSChain.RemoveData(const Data: Pointer);
var
  Cur: PCSLink;
begin
  if m_Count = 0 then Exit;
  Cur := m_First;
  if Cur^.Data = Data then RemoveLink(Cur);
  while Assigned(Cur^.Next) do
  begin
    Cur := Cur^.Next;
    if Cur^.Data = Data then RemoveLink(Cur);
  end;
end;

procedure TCSChain.RemoveLink(const Link: PCSLink);
begin
  if m_Current = Link then m_Current := Link.Prev;
  if m_First = Link then m_First := m_First.Next;
  if m_Last = Link then m_Last := Link.Prev;
  if Assigned(Link^.Prev) and Assigned(Link^.Next) then
  begin
    Link^.Prev^.Next := Link^.Next;
    Link^.Next^.Prev := Link^.Prev;
  end
  else
  if Assigned(Link^.Next) then
  Link^.Next^.Prev := nil
  else
  if Assigned(Link^.Prev) then
  Link^.Prev^.Next := nil;
  Dispose(Link);
  m_Count := m_Count - 1;
end;

procedure TCSChain.RemoveCurrent;
begin
  if m_Current = nil then Exit;
  RemoveLink(m_Current);
end;

procedure TCSChain.Reset;
begin
  m_Current := nil;
end;

procedure TCSChain.Clear;
var
  CurLink, KillLink: PCSLink;
begin
  if m_Count = 0 then Exit;
  CurLink := m_First;
  while Assigned(CurLink^.Next) do
  begin
    KillLink := CurLink;
    CurLink := CurLink^.Next;
    Dispose(KillLink);
  end;
  Dispose(CurLink);
  m_Count := 0;
  m_First := nil;
  m_Current := nil;
  m_Last := nil;
end;
//TCSChain END

//TCSWorld BEGIN
constructor TCSWorld.Create;
begin
  inherited Create;
  m_Scene := TCSScene.Create;
  m_Scene.m_World := Self;
  m_Bodies := TCSChain.Create;
  m_BodiesDisabled := TCSChain.Create;
  m_Coly := TCSColy.Create;
  m_Dyna := TCSDyna.Create;
  m_ContactGroups := TList.Create;
  m_Initialized := false;
end;

destructor TCSWorld.Destroy;
begin
  FreeContactGroups;
  m_ContactGroups.Free;
  m_Dyna.Free;
  m_Coly.Free;
  FreeBodies;
  m_Bodies.Free;
  m_BodiesDisabled.Free;
  m_Scene.Destroy;
  inherited Destroy;
end;

function TCSWorld.AddContactGroup(
      const Body1, Body2: TCSBody;
      const Contacts: TCSchain;
      const R: TCSResponse
    ): PCSContactGroup;
var
  i: integer;
begin
  for i := 0 to m_ContactGroups.Count - 1 do
  if (
    (PCSContactGroup(m_ContactGroups.Items[i])^.Body1 = Body1)
    and (PCSContactGroup(m_ContactGroups.Items[i])^.Body2 = Body2)
  ) or (
    (PCSContactGroup(m_ContactGroups.Items[i])^.Body1 = Body2)
    and (PCSContactGroup(m_ContactGroups.Items[i])^.Body2 = Body1)
  ) then
  begin
    Result := nil;
    Exit;
  end;
  New(Result);
  Result^.Body1 := Body1;
  Result^.Body2 := Body2;
  Result^.Contacts := Contacts;
  Result^.R := R;
  m_ContactGroups.Add(Result);
end;

procedure TCSWorld.Collide(Body1, Body2: TCSBody);
var
  cg: PCSContactGroup;
  vd: TCSVector;
  d1, d2: single;
  Proceed: boolean;
  Contacts: TCSChain;
  R: TCSResponse;
begin
  if Body1.m_IsStatic and Body2.m_IsStatic then Exit;
  if Assigned(m_PreColyCallback) then
  m_PreColyCallback(Body1, Body2, Proceed)
  else
  Proceed := true;
  if Proceed then
  begin
    vd := Body1.m_Pos - Body2.m_Pos;
    d1 := vd * vd;
    d2 := Body1.m_Rad + Body2.m_Rad;
    d2 := d2 * d2;
    if d1 <= d2 then
    begin
      Contacts := TCSChain.Create;
      if m_Coly.CollideShapes(Body1.Shape, Body2.Shape, Contacts, R) then
      begin
        cg := AddContactGroup(Body1, Body2, Contacts, R);
      end
      else
      begin
        ClearContactChain(Contacts);
        Contacts.Free;
      end;
    end;
  end;
end;

procedure TCSWorld.InitWorld(
      const X, Y, SizeX, SizeY: single;
      const Depth: WORD = 4
    );
var
  d: WORD;
begin
  if m_Initialized then UnInit;
  if Depth > 6 then d := 6 else d := Depth;
  m_Scene.Init(X, Y, SizeX, SizeY, d);
  m_Bodies.Reset;
  while m_Bodies.Next <> nil do
  begin
    TCSBody(m_Bodies.Current).m_SceneObject := TCSSceneObject.Create;
    m_Scene.AddObject(TCSBody(m_Bodies.Current).m_SceneObject);
  end;
  m_BodiesDisabled.Reset;
  while m_BodiesDisabled.Next <> nil do
  begin
    TCSBody(m_BodiesDisabled.Current).m_SceneObject := TCSSceneObject.Create;
    m_Scene.AddObject(TCSBody(m_BodiesDisabled.Current).m_SceneObject);
  end;
  m_Initialized := true;
end;

procedure TCSWorld.UnInit;
begin
  m_Scene.UnInit;
  m_Initialized := false;
end;

function TCSWorld.CreateBody: TCSBody;
begin
  Result := TCSBody.Create;
  Result.m_World := Self;
  Result.m_SceneObject := TCSSceneObject.Create;
  Result.m_SceneObject.m_Body := Result;
  Result.m_Link := m_Bodies.AddData(Result);
  m_Scene.AddObject(Result.m_SceneObject);
end;

procedure TCSWorld.DestroyBody(const Body: TCSBody);
begin
  m_Bodies.RemoveLink(Body.m_Link);
  Body.m_SceneObject.Remove;
  Body.m_SceneObject.Free;
  Body.Free;
end;

procedure TCSWorld.FreeBodies;
begin
  m_Bodies.Reset;
  while m_Bodies.Next <> nil do
  TCSBody(m_Bodies.Current).Free;
  m_Bodies.Clear;
  m_BodiesDisabled.Reset;
  while m_BodiesDisabled.Next <> nil do
  TCSBody(m_BodiesDisabled.Current).Free;
  m_BodiesDisabled.Clear;
end;

procedure TCSWorld.FreeContactGroups;
begin
  while m_ContactGroups.Count > 0 do
  begin
    ClearContactChain(PCSContactGroup(m_ContactGroups.Items[0])^.Contacts);
    PCSContactGroup(m_ContactGroups.Items[0])^.Contacts.Free;
    Dispose(PCSContactGroup(m_ContactGroups.Items[0]));
    m_ContactGroups.Delete(0);
  end;
end;

procedure TCSWorld.Step(const StepSize: single);
var
  i: integer;
begin
  m_Bodies.Reset;
  while m_Bodies.Next <> nil do
  begin
    if Assigned(m_ProcCallback) then
    m_ProcCallback(TCSBody(m_Bodies.Current));
    TCSBody(m_Bodies.Current).Step(StepSize);
  end;

  FreeContactGroups;

  m_Bodies.Reset;
  while m_Bodies.Next <> nil do
  m_Scene.QueryCollision(TCSBody(m_Bodies.Current));

  for i := 0 to m_ContactGroups.Count - 1 do
  begin
    m_Dyna.ResolveContacts(
      PCSContactGroup(m_ContactGroups.Items[i])^.Body1,
      PCSContactGroup(m_ContactGroups.Items[i])^.Body2,
      PCSContactGroup(m_ContactGroups.Items[i])^.Contacts,
      PCSContactGroup(m_ContactGroups.Items[i])^.R
    );
    if Assigned(m_ColyCallback)
    and (PCSContactGroup(m_ContactGroups.Items[i])^.Contacts.Count > 0) then
    m_ColyCallback(
      PCSContactGroup(m_ContactGroups.Items[i])^.Body1,
      PCSContactGroup(m_ContactGroups.Items[i])^.Body2,
      PCSContactGroup(m_ContactGroups.Items[i])^.Contacts,
      PCSContactGroup(m_ContactGroups.Items[i])^.R
    );
  end;
end;
//TCSWorld END

//TCSScene BEGIN
constructor TCSScene.Create;
begin
  inherited Create;
  m_ParentTree := TCSSceneTree.Create;
  m_QueryDown := false;
end;

destructor TCSScene.Destroy;
begin
  m_ParentTree.Free;
  inherited Destroy;
end;

procedure TCSScene.Init(
      const X, Y, SizeX, SizeY: single;
      const Depth: WORD
    );
  procedure InitSubTrees(const Tree: TCSSceneTree; const CurTree: WORD);
  var
    i: integer;
  begin
    Tree.Init;
    if CurTree + 1 < m_Depth then
    for i := 0 to High(Tree.m_SubTrees) do
    InitSubTrees(TCSSceneTree(Tree.m_SubTrees[i]), CurTree + 1);
  end;
  procedure PatchTree(const Tree: TCSSceneTree; const CurTree: WORD);
  var
    i: integer;
  begin
    Tree.PatchTree;
    if CurTree + 2 < m_Depth then
    for i := 0 to High(Tree.m_SubTrees) do
    PatchTree(Tree.m_SubTrees[i], CurTree + 1);
  end;
begin
  m_ParentTree.m_Bounds := CSSceneRect(X, Y, X + SizeX, Y + SizeY);
  m_Depth := Depth;
  InitSubTrees(m_ParentTree, 0);
  //if m_Depth > 1 then
  //PatchTree(m_ParentTree, 0);
end;

procedure TCSScene.UnInit;
begin
  m_ParentTree.FreeObjects;
  m_ParentTree.FreeSubTrees;
end;

procedure TCSScene.AddObject(Obj: TCSSceneObject);
begin
  m_ParentTree.AddObject(Obj);
end;

procedure TCSScene.QueryCollision(Body: TCSBody);
  procedure QueryTree(Tree: TCSSceneTree);
  var
    i: integer;
  begin
    for i := 0 to Tree.m_Objects.Count - 1 do
    if TCSSceneObject(Tree.m_Objects.Items[i]).m_Body <> Body then
    m_World.Collide(Body, TCSSceneObject(Tree.m_Objects.Items[i]).m_Body);
  end;
  procedure QueryTreeUp(Tree: TCSSceneTree);
  var
    i, j: integer;
  begin
    QueryTree(Tree);
    if Assigned(Tree.m_Parent) then
    begin
      for j := 0 to High(Tree.m_Parent.m_SubTrees) do
      for i := 0 to Tree.m_Parent.m_SubTrees[j].m_Objects.Count - 1 do
      QueryTree(Tree.m_Parent.m_SubTrees[j]);
      QueryTreeUp(Tree.m_Parent);
    end;
  end;
  procedure QueryTreeDown(Tree: TCSSceneTree);
  var
    i: integer;
  begin
    QueryTree(Tree);
    for i := 0 to High(Tree.m_SubTrees) do
    QueryTreeDown(Tree.m_SubTrees[i]);
  end;
var
  i: integer;
begin
  if Assigned(Body.m_SceneObject.m_Tree) then
  begin
    if m_QueryDown then
    QueryTreeDown(Body.m_SceneObject.m_Tree)
    else
    QueryTree(Body.m_SceneObject.m_Tree);
    if Assigned(Body.m_SceneObject.m_Tree.m_Parent) then
    QueryTreeUp(Body.m_SceneObject.m_Tree.m_Parent);
    for i := 0 to High(Body.m_SceneObject.m_Tree.m_UpLinks) do
    QueryTreeUp(Body.m_SceneObject.m_Tree.m_UpLinks[i]);
  end;
end;
//TCSScene END

//TCSSceneTree BEGIN
constructor TCSSceneTree.Create;
begin
  inherited Create;
  m_Objects := TList.Create;
  m_TreeType := ttRandom;
  m_Patch := false;
end;

destructor TCSSceneTree.Destroy;
begin
  FreeObjects;
  m_Objects.Free;
  FreeSubTrees;
  inherited Destroy;
end;

procedure TCSSceneTree.CreateSubTrees(const Count: integer);
var
  i: integer;
begin
  SetLength(m_SubTrees, Count);
  for i := 0 to High(m_SubTrees) do
  begin
    m_SubTrees[i] := TCSSceneTree.Create;
    m_SubTrees[i].m_Parent := Self;
  end;
end;

function TCSSceneTree.MakeSubTree: TCSSceneTree;
begin
  Result := TCSSceneTree.Create;
  Result.m_Parent := Self;
  AddTree(Result);
end;

function TCSSceneTree.TryFit(Obj: TCSSceneObject): boolean;
begin
  Result := (
    (Obj.m_Pos.x - Obj.m_Radius >= m_Bounds.X1)
    and
    (Obj.m_Pos.y - Obj.m_Radius >= m_Bounds.Y1)
    and
    (Obj.m_Pos.x + Obj.m_Radius < m_Bounds.X2)
    and
    (Obj.m_Pos.y + Obj.m_Radius < m_Bounds.Y2)
  );
end;

function TCSSceneTree.AddTree(Tree: TCSSceneTree): boolean;
var
  i: integer;
begin
  for i := 0 to High(m_SubTrees) do
  if Tree = m_SubTrees[i] then
  begin
    Result := false;
    Exit;
  end;
  SetLength(m_SubTrees, Length(m_SubTrees) + 1);
  m_SubTrees[High(m_SubTrees)] := Tree;
  SetLength(Tree.m_UpLinks, Length(Tree.m_UpLinks) + 1);
  Tree.m_UpLinks[High(Tree.m_UpLinks)] := Self;
  Result := true;
end;

procedure TCSSceneTree.PatchTree;
var
  Patch: TCSSceneTree;
begin
  case m_TreeType of
    ttXDiv:
    begin
      if (m_SubTrees[0].m_TreeType = ttXDiv)
      and (m_SubTrees[1].m_TreeType = ttXDiv) then
      begin
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttXDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[1]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[0]);
        Patch.DetectBounds;
      end
      else if (m_SubTrees[0].m_TreeType = ttQuadDiv)
      and (m_SubTrees[1].m_TreeType = ttQuadDiv) then
      begin
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[1]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[0]);
        Patch.AddTree(m_SubTrees[0].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[2]);
        Patch.DetectBounds;
      end;
    end;
    ttYDiv:
    begin
      if (m_SubTrees[0].m_TreeType = ttYDiv)
      and (m_SubTrees[1].m_TreeType = ttYDiv) then
      begin
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttYDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[1]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[0]);
        Patch.DetectBounds;
      end
      else if (m_SubTrees[0].m_TreeType = ttQuadDiv)
      and (m_SubTrees[1].m_TreeType = ttQuadDiv) then
      begin
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[2]);
        Patch.AddTree(m_SubTrees[0].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[0]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[1]);
        Patch.DetectBounds;
      end;
    end;
    ttQuadDiv:
    begin
      if (m_SubTrees[0].m_TreeType = ttQuadDiv)
      and (m_SubTrees[1].m_TreeType = ttQuadDiv)
      and (m_SubTrees[2].m_TreeType = ttQuadDiv)
      and (m_SubTrees[3].m_TreeType = ttQuadDiv) then
      begin
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[2]);
        Patch.AddTree(m_SubTrees[2].m_SubTrees[1]);
        Patch.AddTree(m_SubTrees[3].m_SubTrees[0]);
        Patch.DetectBounds;
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[1]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[0]);
        Patch.AddTree(m_SubTrees[0].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[2]);
        Patch.DetectBounds;
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[0].m_SubTrees[2]);
        Patch.AddTree(m_SubTrees[0].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[2].m_SubTrees[0]);
        Patch.AddTree(m_SubTrees[2].m_SubTrees[1]);
        Patch.DetectBounds;
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[1].m_SubTrees[2]);
        Patch.AddTree(m_SubTrees[1].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[3].m_SubTrees[0]);
        Patch.AddTree(m_SubTrees[3].m_SubTrees[1]);
        Patch.DetectBounds;
        Patch := MakeSubTree;
        Patch.m_Patch := true;
        Patch.m_TreeType := ttQuadDiv;
        Patch.AddTree(m_SubTrees[2].m_SubTrees[1]);
        Patch.AddTree(m_SubTrees[3].m_SubTrees[0]);
        Patch.AddTree(m_SubTrees[2].m_SubTrees[3]);
        Patch.AddTree(m_SubTrees[3].m_SubTrees[2]);
        Patch.DetectBounds;
      end;
    end;
  end;
end;

procedure TCSSceneTree.DetectBounds;
var
  i: integer;
begin
  if Length(m_SubTrees) = 0 then Exit;
  m_Bounds.X1 := m_SubTrees[0].Bounds.X1;
  m_Bounds.Y1 := m_SubTrees[0].Bounds.Y1;
  m_Bounds.X2 := m_SubTrees[0].Bounds.X2;
  m_Bounds.Y2 := m_SubTrees[0].Bounds.Y2;
  for i := 1 to High(m_SubTrees) do
  begin
    if m_SubTrees[i].Bounds.X1 < m_Bounds.X1 then
    m_Bounds.X1 := m_SubTrees[i].Bounds.X1;
    if m_SubTrees[i].Bounds.Y1 < m_Bounds.Y1 then
    m_Bounds.Y1 := m_SubTrees[i].Bounds.Y1;
    if m_SubTrees[i].Bounds.X2 > m_Bounds.X2 then
    m_Bounds.X2 := m_SubTrees[i].Bounds.X2;
    if m_SubTrees[i].Bounds.Y2 > m_Bounds.Y2 then
    m_Bounds.Y2 := m_SubTrees[i].Bounds.Y2;
  end;
end;

procedure TCSSceneTree.FreeObjects;
begin
  while m_Objects.Count > 0 do
  begin
    TCSSceneObject(m_Objects.Items[0]).Free;
    m_Objects.Delete(0);
  end;
end;

procedure TCSSceneTree.FreeSubTrees;
var
  i: integer;
  Lim: integer;
begin
  if m_Patch then
  begin
    if (m_TreeType = ttXDiv) or (m_TreeType = ttYDiv) then
    Lim := 2;
    if (m_TreeType = ttQuadDiv) then
    Lim := 4;
  end
  else
  Lim := 0;
  for i := Lim to High(m_SubTrees) do
  m_SubTrees[i].Free;
  SetLength(m_SubTrees, 0);
end;

procedure TCSSceneTree.Init;        
var
  W, H: single;
begin
  W := m_Bounds.X2 - m_Bounds.X1;
  H := m_Bounds.Y2 - m_Bounds.Y1;
  if W > H * 2 then
  begin
    m_TreeType := ttXDiv;
    W := W * 0.5;
    CreateSubTrees(2);
    TCSSceneTree(m_SubTrees[0]).m_Bounds := CSSceneRect(
      m_Bounds.X1, m_Bounds.Y1, m_Bounds.X1 + W, m_Bounds.Y1 + H
    );
    TCSSceneTree(m_SubTrees[1]).m_Bounds := CSSceneRect(
      m_Bounds.X1 + W, m_Bounds.Y1, m_Bounds.X2, m_Bounds.Y1 + H
    );
  end
  else if H > W * 2 then
  begin
    m_TreeType := ttYDiv;
    H := H * 0.5;
    CreateSubTrees(2);
    TCSSceneTree(m_SubTrees[0]).m_Bounds := CSSceneRect(
      m_Bounds.X1, m_Bounds.Y1, m_Bounds.X1 + W, m_Bounds.Y1 + H
    );
    TCSSceneTree(m_SubTrees[1]).m_Bounds := CSSceneRect(
      m_Bounds.X1, m_Bounds.Y1 + H, m_Bounds.X1 + W, m_Bounds.Y2
    );
  end
  else
  begin
    m_TreeType := ttQuadDiv;
    W := W * 0.5;
    H := H * 0.5;
    CreateSubTrees(4);
    TCSSceneTree(m_SubTrees[0]).m_Bounds := CSSceneRect(
      m_Bounds.X1, m_Bounds.Y1, m_Bounds.X1 + W, m_Bounds.Y1 + H
    );
    TCSSceneTree(m_SubTrees[1]).m_Bounds := CSSceneRect(
      m_Bounds.X1 + W, m_Bounds.Y1, m_Bounds.X2, m_Bounds.Y1 + H
    );
    TCSSceneTree(m_SubTrees[2]).m_Bounds := CSSceneRect(
      m_Bounds.X1, m_Bounds.Y1 + H, m_Bounds.X1 + W, m_Bounds.Y2
    );
    TCSSceneTree(m_SubTrees[3]).m_Bounds := CSSceneRect(
      m_Bounds.X1 + W, m_Bounds.Y1 + H, m_Bounds.X2, m_Bounds.Y2
    );
  end;
end;

procedure TCSSceneTree.AddObject(Obj: TCSSceneObject);
var
  i: integer;
  b: boolean;
begin
  if TryFit(Obj) then
  begin
    b := false;
    for i := 0 to High(m_SubTrees) do
    if TCSSceneTree(m_SubTrees[i]).TryFit(Obj) then
    begin
      TCSSceneTree(m_SubTrees[i]).AddObject(Obj);
      b := true;
      Break;
    end;
    if not b then
    begin
      m_Objects.Add(Obj);
      Obj.m_Tree := Self;
    end;
  end
  else
  begin
    if Assigned(m_Parent) then
    m_Parent.AddObject(Obj)
    else
    begin
      m_Objects.Add(Obj);
      Obj.m_Tree := Self;
    end;
  end;
end;
//TCSSceneTree END

//TCSSceneObject BEGIN
constructor TCSSceneObject.Create;
begin
  inherited Create;
  m_Pos := CSVector(0, 0);
  m_Radius := 0;
  m_Tree := nil;
end;

destructor TCSSceneObject.Destroy;
begin
  inherited Destroy;
end;

procedure TCSSceneObject.ReAllocate;
var
  i: integer;
  b: boolean;
begin
  if not Assigned(m_Tree) then Exit;
  b := false;
  if m_Tree.TryFit(Self) then
  begin
    for i := 0 to High(m_Tree.m_SubTrees) do
    if m_Tree.m_SubTrees[i].TryFit(Self) then
    b := true;
  end
  else
  b := true;
  if b then
  begin
    m_Tree.m_Objects.Remove(Self);
    m_Tree.AddObject(Self);
  end;
end;

procedure TCSSceneObject.SetPos(Value: TCSVector);
begin
  m_Pos := Value;
  ReAllocate;
end;

procedure TCSSceneObject.SetRadius(Value: single);
begin
  m_Radius := Value;
  ReAllocate;
end;

procedure TCSSceneObject.Remove;
begin
  if Assigned(m_Tree) then
  m_Tree.m_Objects.Remove(Self);
  m_Tree := nil;
end;
//TCSSceneObject END

//TCSColy BEGIN
constructor TCSColy.Create;
begin
  inherited Create;
end;

destructor TCSColy.Destroy;
begin
  inherited Destroy;
end;

function TCSColy.CollideShapes(
      const S1, S2: TCSShape;
      const Contacts: TCSChain;
      var R: TCSResponse
    ): boolean;
var
  ContactArr: TCSContactArray;
  Contact: PCSContact;
  ResponseAcc: TCSVector;
  Response: TCSResponse;
  i: integer;
begin
  Result := false;
  ResponseAcc := CSVector(0, 0);
  S1.m_Prims.Reset;
  while S1.m_Prims.Next <> nil do
  begin
    S2.m_Prims.Reset;
    while S2.m_Prims.Next <> nil do
    begin
      if CollidePrims(
        TCSPrim(S1.m_Prims.Current),
        TCSPrim(S2.m_Prims.Current),
        ContactArr, Response
      ) then
      begin
        Result := true;
        ResponseAcc := ResponseAcc + Response.N * Response.D;
        for i := 0 to High(ContactArr) do
        begin
          New(Contact);
          Contact^.Pos := ContactArr[i].Pos;
          Contact^.N1 := ContactArr[i].N1;
          Contact^.N2 := ContactArr[i].N2;
          Contact^.Mag := ContactArr[i].Mag;
          Contacts.AddData(Contact);
        end;
      end;
    end;
  end;
  R.N := CSVecNormalize(ResponseAcc);
  R.D := CSVecLength(ResponseAcc);
  if R.D < 0 then
  begin
    Result := false;
    R.D := 0;
    R.N := CSVector(0, 0);
  end;
end;

function TCSColy.CollidePrims(
      const P1, P2: TCSPrim;
      var Contacts: TCSContactArray;
      var R: TCSResponse
    ): boolean;
begin
  if (P1.m_PrimType = ptPoly) and (P2.m_PrimType = ptPoly) then
  begin
    Result := CSPolyIntersect(
      TCSPrimPolygon(P1).m_Polygon,
      TCSPrimPolygon(P2).m_Polygon,
      @R
    );
    if Result then
    Contacts := CSPolyFindContacts(
      R.N,
      TCSPrimPolygon(P1).m_Polygon,
      TCSPrimPolygon(P2).m_Polygon
    );
  end
  else if (P1.m_PrimType = ptCircle) and (P2.m_PrimType = ptCircle) then
  begin
    Result := CSCircleIntersect(
      TCSPrimCircle(P1).m_Circle,
      TCSPrimCircle(P2).m_Circle,
      @R
    );
    if Result then
    Contacts := CSCircleFindContacts(
      R.N,
      TCSPrimCircle(P1).m_Circle,
      TCSPrimCircle(P2).m_Circle
    );
  end
  else if (P1.m_PrimType = ptPoly) and (P2.m_PrimType = ptCircle) then
  begin
    Result := CSPolyVsCircleIntersect(
      TCSPrimPolygon(P1).m_Polygon,
      TCSPrimCircle(P2).m_Circle,
      @R
    );
    if Result then
    Contacts := CSPolyVsCircleFindContacts(
      R.N,
      TCSPrimPolygon(P1).m_Polygon,
      TCSPrimCircle(P2).m_Circle
    );
  end
  else if (P1.m_PrimType = ptCircle) and (P2.m_PrimType = ptPoly) then
  begin
    Result := CSCircleVsPolyIntersect(
      TCSPrimCircle(P1).m_Circle,
      TCSPrimPolygon(P2).m_Polygon,
      @R
    );
    if Result then
    Contacts := CSCircleVsPolyFindContacts(
      R.N,
      TCSPrimCircle(P1).m_Circle,
      TCSPrimPolygon(P2).m_Polygon
    );
  end;
end;
//TCSColy END

//TCSDyna BEGIN
constructor TCSDyna.Create;
begin
  inherited Create;
end;

destructor TCSDyna.Destroy;
begin
  inherited Destroy;
end;

function TCSDyna.GetForceAtPoint(
      const Body: TCSBody;
      const P: TCSVector
    ): TCSVector;
var
  ModelPoint: TCSVector;
begin
  if Body.m_IsStatic then
  begin
    Result := CSVector(0, 0);
    Exit;
  end;
  ModelPoint := P - Body.m_Pos;
  Result := (
    (
      CSVector(- ModelPoint.y, ModelPoint.x) * Body.m_VelAngular +
      Body.m_VelLinear
    ) * Body.m_Mass
  );
end;

procedure TCSDyna.SetForceAtPoint(
      const Body: TCSBody;
      const P, F: TCSVector
    );
var
  ModelPoint: TCSVector;
  R, D, grad: single;
  Perp, V: TCSVector;
begin
  if Body.m_IsStatic then Exit;
  ModelPoint := P - Body.m_Pos;

  V := F * Body.m_MassInv;

  grad := Abs(CSVecNormalize(ModelPoint) * CSVecNormalize(V));
  Body.m_VelLinear := Body.m_VelLinear + V * grad;

  D := CSVecLength(ModelPoint);
  if D <= 1 then Exit;
  Perp := CSVecNormalize(CSVector(-ModelPoint.y, ModelPoint.x));
  R := (Perp * V) / D;
  Body.m_VelAngular := Body.m_VelAngular + R;
end;

procedure TCSDyna.ResolveContacts(
      const Body1, Body2: TCSBody;
      const Contacts: TCSChain;
      const R: TCSResponse
    );
var
  TM, M1, M2, TH, TF: single;
  FTotal1, FDirect1, FFriction1: TCSVector;
  FTotal2, FDirect2, FFriction2: TCSVector;
  F: TCSVector;
  C: PCSContact;
begin
  if Body1.m_IsStatic then
  begin
    M1 := 0;
    M2 := 1;
  end
  else
  if Body2.m_IsStatic then
  begin
    M1 := 1;
    M2 := 0;
  end
  else
  begin
    if Body1.m_Mass + Body2.m_Mass > 0 then
    begin
      TM := 1 / (Body1.m_Mass + Body2.m_Mass);
      M1 := Body1.m_Mass * TM;
      M2 := Body2.m_Mass * TM;
    end
    else
    begin
      M1 := 0.5;
      M2 := 0.5;
    end;
  end;
  Body1.m_ImpLinear := Body1.m_ImpLinear + R.N * R.D * M1;
  Body2.m_ImpLinear := Body2.m_ImpLinear - R.N * R.D * M2;

  TH := Body1.m_Hardness * Body2.m_Hardness;
  TF := Body1.m_Friction * Body2.m_Friction;

  Contacts.Reset;
  while Contacts.Next <> nil do
  begin
    C := PCSContact(Contacts.Current);
    FTotal1 := GetForceAtPoint(Body1, C^.Pos);
    FTotal2 := GetForceAtPoint(Body2, C^.Pos);
    ResolveForces(
      C^.N2, C^.N1,
      FTotal1, FTotal2,
      FDirect1, FFriction1,
      FDirect2, FFriction2,
      TH, TF
    );
    begin
      F := (FDirect2 + FFriction2 - FDirect1 - FFriction1) * C^.Mag;
      SetForceAtPoint(Body1, C^.Pos, F);
      SetForceAtPoint(Body2, C^.Pos, -F);
    end;
  end;
end;

function TCSDyna.ResolveForces(
      const N1: TCSVector;
      const N2: TCSVector;
      const F1: TCSVector;
      const F2: TCSVector;
      var Direct1: TCSVector;
      var Friction1: TCSVector;
      var Direct2: TCSVector;
      var Friction2: TCSVector;
      const MagHardness: single = 1;
      const MagFriction: single = 1
    ): boolean;
var
  FD1, FD2, FF1, FF2, FDR, FS1, FS2: single;
  NF1, NF2: TCSVector;
begin
  FD1 := N1 * F1;
  FD2 := N2 * F2;
  FDR := FD1 + FD2;
  if FDR <= 0 then
  begin
    Direct1 := CSVector(0, 0);
    Friction1 := CSVector(0, 0);
    Direct2 := CSVector(0, 0);
    Friction2 := CSVector(0, 0);
    Result := false;
    Exit;
  end;
  NF1 := CSVector(-N1.y, N1.x);
  NF2 := -NF1;
  FF1 := (F1 * NF1);
  FF2 := (F2 * NF2);
  if FF1 < 0 then
  begin
    NF1 := -NF1;
    FF1 := -FF1;
  end;
  if FF2 < 0 then
  begin
    NF2 := -NF2;
    FF2 := -FF2;
  end;
  Direct1 := N1 * (FD1 * MagHardness);
  Direct2 := N2 * (FD2 * MagHardness);
  Friction1 := NF1 * (FF1 * MagFriction);
  Friction2 := NF2 * (FF2 * MagFriction);
  Result := true;
end;
//TCSDyna END

//TCSPrim BEGIN
constructor TCSPrim.Create;
begin
  inherited Create;
end;

destructor TCSPrim.Destroy;
begin
  inherited Destroy;
end;

procedure TCSPrim.TransformRotate(const Angle: single);
begin
  TransformRotate(CSMatrixRotation(Angle));
end;
//TCSPrim END

//TCSPrimCircle BEGIN
constructor TCSPrimCircle.Create;
begin
  inherited Create;
  m_PrimType := ptCircle;
end;

function TCSPrimCircle.GetCenter: TCSVector;
begin
  Result := m_Circle.Pos;
end;

function TCSPrimCircle.GetCircle: PCSCircle;
begin
  Result := @m_Circle;
end;

procedure TCSPrimCircle.TransformMove(const Pos: TCSVector);
begin
  m_Circle.Pos := m_Circle.Pos + Pos;
end;

procedure TCSPrimCircle.TransformRotate(const m: TCSMatrix2);
begin
  m_Circle.Pos := m_Circle.Pos * m;
end;

function TCSPrimCircle.Duplicate: TCSPrim;
begin
  Result := TCSPrimCircle.Create;
  Move(m_Circle, TCSPrimCircle(Result).m_Circle, SizeOf(TCSCircle));
end;

function TCSPrimCircle.GetRadius: single;
begin
  Result := m_Circle.Radius;
end;
//TCSPrimCircle END

//TCSPrimPolygon BEGIN
constructor TCSPrimPolygon.Create;
begin
  inherited Create;
  m_PrimType := ptPoly;
end;

function TCSPrimPolygon.GetCenter: TCSVector;
begin
  Result := CSPolyCenter(m_Polygon);
end;

function TCSPrimPolygon.GetPolygon: PCSPolygon;
begin
  Result := @m_Polygon;
end;

procedure TCSPrimPolygon.TransformMove(const Pos: TCSVector);
begin
  CSPolyMove(m_Polygon, Pos);
end;

procedure TCSPrimPolygon.TransformRotate(const m: TCSMatrix2);
begin
  CSPolyTransform(m_Polygon, m);
end;

function TCSPrimPolygon.Duplicate: TCSPrim;
begin
  Result := TCSPrimPolygon.Create;
  SetLength(TCSPrimPolygon(Result).m_Polygon, Length(m_Polygon));
  Move(m_Polygon[0], TCSPrimPolygon(Result).m_Polygon[0], SizeOf(TCSVector) * Length(m_Polygon));
end;

function TCSPrimPolygon.GetRadius: single;
var
  r: single;
  i: integer;
  c: TCSVector;
begin
  c := CSPolyCenter(m_Polygon);
  Result := CSVecLength(m_Polygon[0] - c);
  for i := 1 to High(m_Polygon) do
  begin
    r := CSVecLength(m_Polygon[i] - c);
    if r > Result then Result := r;
  end;
end;
//TCSPrimPolygon END

//TCSShape BEGIN
constructor TCSShape.Create;
begin
  inherited Create;
  m_Prims := TCSChain.Create;
  m_Radius := 0;
end;

destructor TCSShape.Destroy;
begin
  Clear;
  m_Prims.Free;
  inherited Destroy;
end;

procedure TCSShape.AddPrim(const Prim: TCSPrim);
var
  r: single;
begin
  m_Prims.AddData(Prim);
  r := Prim.GetRadius + CSVecLength(Prim.Center);
  if r > m_Radius then m_Radius := r;
end;

procedure TCSShape.AddCircle(const Pos: TCSVector; const Radius: single);
var
  Prim: TCSPrimCircle;
begin
  Prim := TCSPrimCircle.Create;
  Prim.m_Circle.Pos := Pos;
  Prim.m_Circle.Radius := Radius;
  AddPrim(Prim);
end;

procedure TCSShape.AddPolygon(const P: TCSPolygon);
var
  Prim: TCSPrimPolygon;
begin
  Prim := TCSPrimPolygon.Create;
  SetLength(Prim.m_Polygon, Length(P));
  Move(P[0], Prim.m_Polygon[0], SizeOf(TCSVector) * Length(P));
  AddPrim(Prim);
end;

procedure TCSShape.Clear;
begin
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  TCSPrim(m_Prims.Current).Free;
  m_Prims.Clear;
end;

procedure TCSShape.TransformMove(const Pos: TCSVector);
begin
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  TCSPrim(m_Prims.Current).TransformMove(Pos);
end;

procedure TCSShape.TransformRotate(const m: TCSMatrix2);
begin
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  TCSPrim(m_Prims.Current).TransformRotate(m);
end;

procedure TCSShape.TransformRotate(const Angle: single);
begin
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  TCSPrim(m_Prims.Current).TransformRotate(Angle);
end;

procedure TCSShape.AutoCM;
var
  CM: TCSVector;
  r: single;
begin
  CM := CSVector(0, 0);
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  CM := CM + TCSPrim(m_Prims.Current).Center;
  CM := -CM / m_Prims.Count;
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  TCSPrim(m_Prims.Current).TransformMove(CM);
  m_Radius := 0;
  r := 0;
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  begin
    r := (
      TCSPrim(m_Prims.Current).GetRadius +
      CSVecLength(TCSPrim(m_Prims.Current).Center)
    );
    if r > m_Radius then m_Radius := r;
  end;
end;

function TCSShape.Duplicate: TCSShape;
begin
  Result := TCSShape.Create;
  m_Prims.Reset;
  while m_Prims.Next <> nil do
  Result.AddPrim(TCSPrim(m_Prims.Current).Duplicate);
end;
//TCSShape END

//TCSBody BEGIN
constructor TCSBody.Create;
begin
  inherited Create;
  m_ShapeSet := false;
  m_RadSq := 0;
  m_RadSqInv := 0;
  m_Pos := CSVector(0, 0);
  m_Rot := CSMatrixIdentity;
  m_VelLinear := CSVector(0, 0);
  m_VelAngular := 0;
  m_ImpLinear := CSVector(0, 0);
  m_ImpAngular := 0;
  m_AccLinear := CSVector(0, 0);
  m_AccAngular := 0;
  m_Mass := 1;
  m_MassInv := 1 / m_Mass;
  m_Inertia := 1;
  m_InertiaInv := 1 / m_Inertia;
  m_Hardness := 1;
  m_Friction := 1;
end;

destructor TCSBody.Destroy;
begin
  FreeShapes;
  inherited Destroy;
end;

procedure TCSBody.FreeShapes;
begin
  if not m_ShapeSet then Exit;
  m_ShapeInit.Free;
  m_ShapeMod.Free;
end;

procedure TCSBody.SetShape(S: TCSShape);
begin
  FreeShapes;
  m_ShapeInit := S.Duplicate;
  m_ShapeMod := m_ShapeInit.Duplicate;
  m_ShapeSet := true;
  m_SceneObject.Radius := m_ShapeInit.m_Radius;
  m_Rad := m_ShapeInit.m_Radius;
  m_RadSq := m_Rad * m_Rad;
  if m_RadSq > 0 then
  m_RadSqInv := 1 / m_RadSq;
  Inertia := 3 * m_RadSq / 5;
end;

procedure TCSBody.Transform;
var
  Prim1, Prim2: TCSPrim;
  i: integer;
begin
  m_ShapeInit.m_Prims.Reset;
  m_ShapeMod.m_Prims.Reset;
  while (m_ShapeInit.m_Prims.Next <> nil)
  and (m_ShapeMod.m_Prims.Next <> nil) do
  begin
    Prim1 := TCSPrim(m_ShapeInit.m_Prims.Current);
    Prim2 := TCSPrim(m_ShapeMod.m_Prims.Current);
    if Prim1.m_PrimType = ptPoly then
    for i := 0 to High(TCSPrimPolygon(Prim1).m_Polygon) do
    TCSPrimPolygon(Prim2).m_Polygon[i] := TCSPrimPolygon(Prim1).m_Polygon[i] * m_Rot + m_Pos;
    if Prim1.m_PrimType = ptCircle then
    TCSPrimCircle(Prim2).m_Circle.Pos := TCSPrimCircle(Prim1).m_Circle.Pos * m_Rot + m_Pos;
  end;
end;

function TCSBody.GetPos: PCSVector;
begin
  Result := @m_Pos;
end;

function TCSBody.GetRot: PCSMatrix2;
begin
  Result := @m_Rot;
end;

function TCSBody.GetVelLinear: PCSVector;
begin
  Result := @m_VelLinear;
end;

function TCSBody.GetVelAngular: PSingle;
begin
  Result := @m_VelAngular;
end;

function TCSBody.GetAccLinear: PCSVector;
begin
  Result := @m_AccLinear;
end;

function TCSBody.GetAccAngular: PSingle;
begin
  Result := @m_AccAngular;
end;

function TCSBody.GetImpLinear: PCSVector;
begin
  Result := @m_ImpLinear;
end;

function TCSBody.GetImpAngular: PSingle;
begin
  Result := @m_ImpAngular;
end;

function TCSBody.GetAng: single;
begin
  Result := CSMatrixExtractRotation(m_Rot);
end;

procedure TCSBody.SetMass(Value: single);
begin
  m_Mass := Value;
  m_MassInv := 1 / m_Mass;
end;

procedure TCSBody.SetInertia(Value: single);
begin
  m_Inertia := Value;
  m_InertiaInv := 1 / m_Inertia;
end;

procedure TCSBody.SetEnabled(Value: boolean);
begin
  if m_IsEnabled = Value then Exit;
  if m_IsEnabled then m_World.m_Bodies.RemoveLink(m_Link)
  else m_World.m_BodiesDisabled.RemoveLink(m_Link);
  m_IsEnabled := Value;
  if m_IsEnabled then
  m_World.m_Bodies.AddData(Self)
  else
  m_World.m_BodiesDisabled.AddData(Self);
end;

procedure TCSBody.Step(const StepSize: single);
begin
  if m_IsStatic then
  begin
    m_VelLinear := CSVector(0, 0);
    m_VelAngular := 0;
  end
  else
  begin
    m_VelLinear := m_VelLinear + m_AccLinear;
    m_VelAngular := m_VelAngular + m_AccAngular;
    if Abs(m_ImpLinear.x) > 10 then m_ImpLinear.x := 0;
    if Abs(m_ImpLinear.y) > 10 then m_ImpLinear.y := 0;
    m_Pos := m_Pos + m_ImpLinear + m_VelLinear * StepSize;
    m_Rot := CSMatrixMultiply(m_Rot, CSMatrixRotation(m_VelAngular * StepSize + m_ImpAngular));
  end;
  Transform;
  m_SceneObject.Pos := m_Pos;
  m_AccLinear := CSVector(0, 0);
  m_AccAngular := 0;
  m_ImpLinear := CSVector(0, 0);
  m_ImpAngular := 0;
end;
//TCSBody END

//TCSVector BEGIN
class operator TCSVector.Negative(const v1: TCSVector): TCSVector;
begin
  Result.x := -v1.x;
  Result.y := -v1.y;
end;

class operator TCSVector.Add(const v1, v2: TCSVector): TCSVector;
begin
  Result := CSVecAdd(v1, v2);
end;

class operator TCSVector.Add(const v1: TCSVector; const s: single): TCSVector;
begin
  Result.x := v1.x + s;
  Result.y := v1.y + s;
end;

class operator TCSVector.Add(const s: single; const v1: TCSVector): TCSVector;
begin
  Result := v1 + s;
end;

class operator TCSVector.Subtract(const v1, v2: TCSVector): TCSVector;
begin
  Result := CSVecSubtract(v1, v2);
end;

class operator TCSVector.Subtract(const v1: TCSVector; const s: single): TCSVector;
begin
  Result.x := v1.x - s;
  Result.y := v1.y - s;
end;

class operator TCSVector.Multiply(const v1, v2: TCSVector): single;
begin
  Result := CSVecDot(v1, v2);
end;

class operator TCSVector.Multiply(const v1: TCSVector; const s: single): TCSVector;
begin
  Result.x := v1.x * s;
  Result.y := v1.y * s;
end;

class operator TCSVector.Multiply(const v1: TCSVector; const m: TCSMatrix2): TCSVector;
begin
  Result := CSVecTransform(v1, m);
end;

class operator TCSVector.Divide(const v1: TCSVector; const s: single): TCSVector;
begin
  Result.x := v1.x / s;
  Result.y := v1.y / s;
end;
//TCSVector END

//Vector Functions BEGIN
function CSVector(const x, y: single): TCSVector; inline;
begin
  Result.x := x;
  Result.y := y;
end;

function CSVecLength(const v: TCSVector): single; inline;
begin
  Result := Sqrt((v.x * v.x) + (v.y * v.y));
end;

function CSVecAdd(const v1, v2: TCSVector): TCSVector; inline;
begin
  Result.x := v1.x + v2.x;
  Result.y := v1.y + v2.y;
end;

function CSVecSubtract(const v1, v2: TCSVector): TCSVector; inline;
begin
  Result.x := v1.x - v2.x;
  Result.y := v1.y - v2.y;
end;

function CSVecNormalize(const v: TCSVector): TCSVector; inline;
var
  d: single;
begin
  if (v.x = 0) and (v.y = 0) then
  begin
    Result := CSVector(0, 0);
    Exit;
  end;
  d := 1 / Sqrt((v.x * v.x) + (v.y * v.y));
  Result.x := v.x * d;
  Result.y := v.y * d;
end;

function CSVecDot(const v1, v2: TCSVector): single; inline;
begin
  Result := v1.x * v2.x + v1.y * v2.y;
end;

function CSVecCross(const v1, v2: TCSVector): single;
begin
  Result := v1.x * v2.y - v1.y * v2.x;
end;

function CSVecTransform(const v1: TCSVector; const m: TCSMatrix2): TCSVector; inline;
var
  vr: TCSVector;
  i, j: integer;
begin
  vr.x := v1.x * m[0, 0] + v1.y * m[1, 0];
  vr.y := v1.x * m[0, 1] + v1.y * m[1, 1];
  Result := vr;
end;

function CSVecInPolygon(const v1: TCSVector; const P1: TCSPolygon): boolean;
var
  i: integer; 
  pi, pj: TCSVector; 
begin 
  Result := false;
  pj := P1[High(P1)];
  for i := 0 to High(P1) do
  begin 
    pi := P1[i];
    if (
      (
        (
          (pi.y <= v1.y)
          and
          (v1.y < pj.y)
        )
        or
        (
          (pj.y <= v1.y)
          and
          (v1.y < pi.y)
        )
      )
      and
      (
        v1.x < (pj.x - pi.x) * (v1.Y - pi.y) / (pj.y - pi.y) + pi.x
      )
    ) then
    Result := not Result; 
    pj := pi; 
  end; 
end;

function CSVecDistance(const v1, v2: TCSVector): single;
begin
  Result := Sqrt(Sqr(v1.x - v2.x) + Sqr(v1.y - v2.y));
end;
//Vector Functions END

//Matrix Functions BEGIN
function CSMatrixIdentity: TCSMatrix2;
begin
  Result[0, 0] := 1;
  Result[1, 0] := 0;
  Result[0, 1] := 0;
  Result[1, 1] := 1;
end;

function CSMatrixScaling(const ScaleX, ScaleY: single): TCSMatrix2;
begin
  Result[0, 0] := ScaleX;
  Result[1, 0] := 0;
  Result[0, 1] := 0;
  Result[1, 1] := ScaleY;
end;

function CSMatrixRotation(const Angle: single): TCSMatrix2;
var
  s, c: single;
begin
  s := Sin(Angle);
  c := Cos(Angle);
  Result[0, 0] := c;
  Result[1, 0] := -s;
  Result[0, 1] := s;
  Result[1, 1] := c;
end;

function CSMatrixAdd(const m1, m2: TCSMatrix2): TCSMatrix2;
begin
  Result[0, 0] := m1[0, 0] + m2[0, 0];
  Result[1, 0] := m1[1, 0] + m2[1, 0];
  Result[0, 1] := m1[0, 1] + m2[0, 1];
  Result[1, 1] := m1[1, 1] + m2[1, 1];
end;

function CSMatrixMultiply(const m1, m2: TCSMatrix2): TCSMatrix2;
var
  mr: TCSMatrix2;
begin
  mr[0, 0] := m1[0, 0] * m2[0, 0] + m1[1, 0] * m2[0, 1];
  mr[0, 1] := m1[0, 1] * m2[0, 0] + m1[1, 1] * m2[0, 1];
  mr[1, 0] := m1[0, 0] * m2[1, 0] + m1[1, 0] * m2[1, 1];
  mr[1, 1] := m1[0, 1] * m2[1, 0] + m1[1, 1] * m2[1, 1];
  Result := mr;
end;

function CSMatrixScale(const m: TCSMatrix2; s: single): TCSMatrix2;
begin
  Result[0, 0] := m[0, 0] * s;
  Result[1, 0] := m[1, 0] * s;
  Result[0, 1] := m[0, 1] * s;
  Result[1, 1] := m[1, 1] * s;
end;

function CSMatrixDeterminant(const m: TCSMatrix2): single;
begin
  Result := (
    m[0, 0] * m[1, 1] - m[1, 0] * m[0, 1]
  );
end;

function CSMatrixTranspose(const m: TCSMatrix2): TCSMatrix2;
var
  rm: TCSMatrix2;
begin
  rm[1, 0] := m[0, 1];
  rm[0, 1] := m[1, 0];
  Move(rm, Result, SizeOf(TCSMatrix2));
end;

function CSMatrixInverse(const m: TCSMatrix2): TCSMatrix2;
var
  D: single;
  rm: TCSMatrix2;
begin
  D := 1 / CSMatrixDeterminant(m);
  rm[0, 0] := m[1, 1] * D;
  rm[0, 1] := -m[0, 1] * D;
  rm[1, 0] := -m[1, 0] * D;
  rm[1, 1] := m[0, 0] * D;
  Move(rm, Result, SizeOf(TCSMatrix2));
end;

function CSMatrixExtractRotation(const m: TCSMatrix2): single;
  function ArcTan2(const Y, X: Extended): Extended;
  asm
          FLD     Y
          FLD     X
          FPATAN
          FWAIT
  end;
begin
  Result := ArcTan2(m[0, 1], m[0, 0]);
end;
//Matrix Functions END

//Polygon Functions BEGIN
procedure CSPolyEdge(const P: TCSPolygon; const EdgeIndex: integer; var Index1, Index2: integer);
begin
  Index1 := EdgeIndex mod Length(P);
  Index2 := (EdgeIndex + 1) mod Length(P);
end;

function CSPolyEdgeDir(const P: TCSPolygon; const EdgeIndex: integer): TCSVector;
var
  Ind1, Ind2: integer;
begin
  CSPolyEdge(P, EdgeIndex, Ind1, Ind2);
  Result := CSVecNormalize(P[Ind2] - P[Ind1]);
end;

function CSPolyEdgeNormal(const P: TCSPolygon; const EdgeIndex: integer): TCSVector;
var
  EdgeDir: TCSVector;
begin
  EdgeDir := CSPolyEdgeDir(P, EdgeIndex);
  Result := CSVector(-EdgeDir.y, EdgeDir.x);
end;

function CSPolyCenter(const P: TCSPolygon): TCSVector;
var
  i: integer;
begin
  Result := P[0];
  for i := 1 to High(P) do
  Result := Result + P[i];
  Result := Result / Length(P);
end;

procedure CSPolyMove(var P: TCSPolygon; const Pos: TCSVector);
var
  i: integer;
begin
  for i := 0 to High(P) do
  P[i] := P[i] + Pos;
end;

procedure CSPolyTransform(var P: TCSPolygon; const m: TCSMatrix2);
var
  i: integer;
begin
  for i := 0 to High(P) do
  P[i] := P[i] * m;
end;
//Polygon Functions BEGIN

//Utility Function BEGIN
procedure CSPolyProject(
    const Axis: TCSVector;
    const P: TCSPolygon;
    var Min, Max: single
  );
var
  d: single;
  i: integer;
begin
  d := Axis * P[0];
  Min := d;
  Max := d;
  for i := 0 to High(P) do
  begin
    d := Axis * P[i];
    if d < Min then Min := d;
    if d > Max then Max := d;
  end;
end;

procedure CSCircleProject(
      const Axis: TCSVector;
      const C: TCSCircle;
      var Min, Max: single
    );
begin
  Min := Axis * C.Pos;
  Max := Min + C.Radius;
  Min := Min - C.Radius;
end;

function CSPolyAxisSeparate(
      const Axis: TCSVector;
      const P1, P2: TCSPolygon;
      const R: PCSResponse = nil
    ): boolean;
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSPolyProject(Axis, P1, Min1, Max1);
  CSPolyProject(Axis, P2, Min2, Max2);
  Result := (Min1 > Max2) or (Min2 > Max1);
  if Assigned(R) then
  begin
    d1 := Max1 - Min2;
    d2 := Max2 - Min1;
    if d1 < d2 then d := d1 else d := d2;
    R^.N := Axis;
    R^.D := Abs(d);
  end;
end;

function CSCircleAxisSeparate(
      const Axis: TCSVector;
      const C1, C2: TCSCircle;
      const R: PCSResponse = nil
    ): boolean;
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSCircleProject(Axis, C1, Min1, Max1);
  CSCircleProject(Axis, C2, Min2, Max2);
  Result := (Min1 > Max2) or (Min2 > Max1);
  if Assigned(R) then
  begin
    d1 := Max1 - Min2;
    d2 := Max2 - Min1;
    if d1 < d2 then d := d1 else d := d2;
    R^.N := Axis;
    R^.D := Abs(d);
  end;
end;

function CSPolyVsCircleAxisSeparate(
      const Axis: TCSVector;
      const P: TCSPolygon;
      const C: TCSCircle;
      const R: PCSResponse = nil
    ): boolean;
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSPolyProject(Axis, P, Min1, Max1);
  CSCircleProject(Axis, C, Min2, Max2);
  Result := (Min1 > Max2) or (Min2 > Max1);
  if Assigned(R) then
  begin
    d1 := Max1 - Min2;
    d2 := Max2 - Min1;
    if d1 < d2 then d := d1 else d := d2;
    R^.N := Axis;
    R^.D := Abs(d);
  end;
end;

function CSCircleVsPolyAxisSeparate(
      const Axis: TCSVector;
      const C: TCSCircle;
      const P: TCSPolygon;
      const R: PCSResponse = nil
    ): boolean;
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSCircleProject(Axis, C, Min1, Max1);
  CSPolyProject(Axis, P, Min2, Max2);
  Result := (Min1 > Max2) or (Min2 > Max1);
  if Assigned(R) then
  begin
    d1 := Max1 - Min2;
    d2 := Max2 - Min1;
    if d1 < d2 then d := d1 else d := d2;
    R^.N := Axis;
    R^.D := Abs(d);
  end;
end;

function CSPolyIntersect(
      const P1, P2: TCSPolygon;
      const R: PCSResponse = nil
    ): boolean;
var
  i: integer;
  EdgeNormal: TCSVector;
  RespVecArr: array of TCSResponse;
  CurResp: integer;
begin
  Result := true;
  SetLength(RespVecArr, Length(P1) + Length(P2));
  CurResp := 0;
  for i := 0 to High(P1) do
  begin
    EdgeNormal := CSPolyEdgeNormal(P1, i);
    if CSPolyAxisSeparate(EdgeNormal, P1, P2, @RespVecArr[CurResp]) then
    begin
      Result := false;
      Exit;
    end;
    Inc(CurResp);
  end;
  for i := 0 to High(P2) do
  begin
    EdgeNormal := CSPolyEdgeNormal(P2, i);
    if CSPolyAxisSeparate(EdgeNormal, P1, P2, @RespVecArr[CurResp]) then
    begin
      Result := false;
      Exit;
    end;
    Inc(CurResp);
  end;
  if Assigned(R) then
  begin
    R^ := CSFindMTD(@RespVecArr[0], CurResp);
    if (CSPolyCenter(P1) - CSPolyCenter(P2)) * R^.N < 0 then
    R^.N := -R^.N;
  end;
end;

function CSCircleIntersect(
    const C1, C2: TCSCircle;
    const R: PCSResponse = nil
  ): boolean;
var
  Axis: TCSVector;
begin
  Axis := C1.Pos - C2.Pos;
  Axis := CSVecNormalize(Axis);
  Result := not CSCircleAxisSeparate(Axis, C1, C2, R);
end;

function CSPolyVsCircleIntersect(
      const P: TCSPolygon;
      const C: TCSCircle;
      const R: PCSResponse = nil
    ): boolean;
var
  i: integer;
  EdgeNormal: TCSVector;
  RespVecArr: array of TCSResponse;
  CurResp: integer;
  d, md: single;
  MinVecInd: integer;
begin
  Result := true;
  SetLength(RespVecArr, Length(P) + 1);
  CurResp := 0;
  for i := 0 to High(P) do
  begin
    EdgeNormal := CSPolyEdgeNormal(P, i);
    if CSPolyVsCircleAxisSeparate(EdgeNormal, P, C, @RespVecArr[CurResp]) then
    begin
      Result := false;
      Exit;
    end;
    Inc(CurResp);
  end;
  MinVecInd := 0;
  d := CSVecDistance(C.Pos, P[0]);
  md := d;
  for i := 1 to High(P) do
  begin
    d := CSVecDistance(C.Pos, P[i]);
    if d < md then
    begin
      md := d;
      MinVecInd := i;
    end;
  end;
  if CSPolyVsCircleAxisSeparate(CSVecNormalize(C.Pos - P[MinVecInd]), P, C, @RespVecArr[CurResp]) then
  begin
    Result := false;
    Exit;
  end;
  Inc(CurResp);
  if Assigned(R) then
  begin
    R^ := CSFindMTD(@RespVecArr[0], CurResp);
    if (CSPolyCenter(P) - C.Pos) * R^.N < 0 then
    R^.N := -R^.N;
  end;
end;

function CSCircleVsPolyIntersect(
      const C: TCSCircle;
      const P: TCSPolygon;
      const R: PCSResponse = nil
    ): boolean;
var
  i: integer;
  EdgeNormal: TCSVector;
  RespVecArr: array of TCSResponse;
  CurResp: integer;
  d, md: single;
  MinVecInd: integer;
begin
  Result := true;
  SetLength(RespVecArr, Length(P) + 1);
  CurResp := 0;
  for i := 0 to High(P) do
  begin
    EdgeNormal := CSPolyEdgeNormal(P, i);
    if CSPolyVsCircleAxisSeparate(EdgeNormal, P, C, @RespVecArr[CurResp]) then
    begin
      Result := false;
      Exit;
    end;
    Inc(CurResp);
  end;
  MinVecInd := 0;
  d := CSVecDistance(C.Pos, P[0]);
  md := d;
  for i := 1 to High(P) do
  begin
    d := CSVecDistance(C.Pos, P[i]);
    if d < md then
    begin
      md := d;
      MinVecInd := i;
    end;
  end;
  if CSPolyVsCircleAxisSeparate(CSVecNormalize(C.Pos - P[MinVecInd]), P, C, @RespVecArr[CurResp]) then
  begin
    Result := false;
    Exit;
  end;
  Inc(CurResp);
  if Assigned(R) then
  begin
    R^ := CSFindMTD(@RespVecArr[0], CurResp);
    if (C.Pos - CSPolyCenter(P)) * R^.N < 0 then
    R^.N := -R^.N;
  end;
end;      

procedure CSPolyFindSupportPoints(
      const N: TCSVector;
      const P: TCSPolygon;
      const List: TList
    );
var
  i: integer;
  d, pd: single;
const
  Threshold = 1E-1;
begin
  d := P[0] * N;
  List.Add(@P[0]);
  for i := 1 to High(P) do
  begin
    pd := P[i] * N;
    if pd >= d - Threshold then
    begin
      if pd > d + Threshold then
      begin
        List.Clear;
        d := pd;
      end;
      List.Add(@P[i]);
    end;
  end;
end;

function CSPolyFindContacts(
      const N: TCSVector;
      const P1, P2: TCSPolygon
    ): TCSContactArray;
var
  SP1, SP2, CP: TList;
  i: integer;
  d, Min1, Max1, Min2, Max2: single;
  Perp: TCSVector;
  Mag: single;
begin
  SP1 := TList.Create;
  SP2 := TList.Create;
  CP := TList.Create;
  CSPolyFindSupportPoints(-N, P1, SP1);
  CSPolyFindSupportPoints(N, P2, SP2);
  if (SP1.Count = 1) and (SP2.Count = 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := (PCSVector(SP1.Items[0])^ + PCSVector(SP2.Items[0])^) * 0.5;
    Result[0].Mag := 1;
  end
  else
  if (SP1.Count = 1) and (SP2.Count > 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := PCSVector(SP1.Items[0])^;
    Result[0].Mag := 1;
  end
  else
  if (SP1.Count > 1) and (SP2.Count = 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := PCSVector(SP2.Items[0])^;
    Result[0].Mag := 1;
  end
  else
  begin
    Perp := CSVector(- N.y, N.x);
    Min1 := PCSVector(SP1.Items[0])^ * Perp;
    Max1 := Min1;
    Min2 := PCSVector(SP2.Items[0])^ * Perp;
    Max2 := Min2;
    for i := 1 to SP1.Count - 1 do
    begin
      d := PCSVector(SP1.Items[i])^ * Perp;
      if d < Min1 then Min1 := d;
      if d > Max1 then Max1 := d;
    end;
    for i := 1 to SP2.Count - 1 do
    begin
      d := PCSVector(SP2.Items[i])^ * Perp;
      if d < Min2 then Min2 := d;
      if d > Max2 then Max2 := d;
    end;
    for i := 0 to SP2.Count - 1 do
    begin
      d := PCSVector(SP2.Items[i])^ * Perp;
      if (d >= Min1) and (d <= Max1) then
      CP.Add(SP2.Items[i]);
    end;
    for i := 0 to SP1.Count - 1 do
    begin
      d := PCSVector(SP1.Items[i])^ * Perp;
      if (d >= Min2) and (d <= Max2) then
      CP.Add(SP1.Items[i]);
    end;
    SetLength(Result, CP.Count);
    Mag := 1 / CP.Count;
    for i := 0 to CP.Count - 1 do
    begin
      Result[i].N1 := N;
      Result[i].N2 := -N;
      Result[i].Pos := PCSVector(CP.Items[i])^;
      Result[0].Mag := Mag;
    end;
  end;
  SP1.Free;
  SP2.Free;
  CP.Free;
end;

function CSCircleFindContacts(
      const N: TCSVector;
      const C1, C2: TCSCircle
    ): TCSContactArray;
begin
  SetLength(Result, 1);
  Result[0].Pos := C2.Pos + N * C2.Radius;
  Result[0].N1 := N;
  Result[0].N2 := -N;
  Result[0].Mag := 1;
end;

function CSPolyVsCircleFindContacts(
      const N: TCSVector;
      const P: TCSPolygon;
      const C: TCSCircle
    ): TCSContactArray;
var
  SP, CP: TList;
  CSP: TCSVector;
begin
  SP := TList.Create;
  CP := TList.Create;
  CSPolyFindSupportPoints(-N, P, SP);
  CSP := C.Pos + N * C.Radius;
  if (SP.Count = 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := PCSVector(SP.Items[0])^;
    Result[0].Mag := 1;
  end
  else
  if (SP.Count > 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := CSP;
    Result[0].Mag := 1;
  end;
  SP.Free;
  CP.Free;
end;

function CSCircleVsPolyFindContacts(
      const N: TCSVector;
      const C: TCSCircle;
      const P: TCSPolygon
    ): TCSContactArray;
var
  SP, CP: TList;
  CSP: TCSVector;
begin
  SP := TList.Create;
  CP := TList.Create;
  CSPolyFindSupportPoints(N, P, SP);
  CSP := C.Pos - N * C.Radius;
  if (SP.Count = 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := PCSVector(SP.Items[0])^;
    Result[0].Mag := 1;
  end
  else
  if (SP.Count > 1) then
  begin
    SetLength(Result, 1);
    Result[0].N1 := N;
    Result[0].N2 := -N;
    Result[0].Pos := CSP;
    Result[0].Mag := 1;
  end;
  SP.Free;
  CP.Free;
end;

function CSFindMTD(
      const RArr: PCSResponseArray;
      const RCount: integer
    ): TCSResponse;
var
  ResultInd, i: integer;
  md, d: single;
begin
  ResultInd := 0;
  md := RArr^[0].D;
  for i := 1 to RCount - 1 do
  begin
    d := RArr^[i].D;
    if d < md then
    begin
      md := d;
      ResultInd := i;
    end;
  end;
  Result := RArr^[ResultInd];
end;

procedure ClearContactChain(
      const Contacts: TCSChain
    );
begin
  Contacts.Reset;
  while Contacts.Next <> nil do
  Dispose(PCSContact(Contacts.Current));
  Contacts.Clear;
end;

function CSSceneRect(
      const X1, Y1, X2, Y2: single
    ): TCSSceneRect;
begin
  Result.X1 := X1;
  Result.Y1 := Y1;
  Result.X2 := X2;
  Result.Y2 := Y2;
end;

function CSSceneRect(
      const Min, Max: TCSVector
    ): TCSSceneRect;
begin
  Result.Min := Min;
  Result.Max := Max;
end;
//Utility Functions END

end.