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

interface

uses
  Classes,
  Windows,
  Types,
  SysUtils;

type
  TCSWorld = class;
  TCSGeom = class;
  TCSGeomPoly = class;
  TCSGeomCircle = class;
  TCSNode = class;
  TCSBody = class;

  Float = Single;
  PFloat = ^Float;
  FloatArray = array[Word] of Float;
  PFloatArray = ^FloatArray;

  TCSRect = record
    var l, t, r, b: Integer;
    procedure SetValue(const _l, _t, _r, _b: Integer); inline;
    function Width: Integer; inline;
    function Height: Integer; inline;
    function Intersect(const r: TCSRect): Boolean; inline;
  end;

  TCSList = record
  strict private
    m_Arr: array of Pointer;
    m_Count: Integer;
    procedure SetElement(const Index: Integer; const Value: Pointer); inline;
    function GetElement(const Index: Integer): Pointer; inline;
    procedure SetCapacity(const Value: Integer); inline;
    function GetCapacity: Integer; inline;
  public
    property e[const Index: Integer]: Pointer read GetElement write SetElement; default;
    property Count: Integer read m_Count write m_Count;
    property Capacity: Integer read GetCapacity write SetCapacity;
    procedure Add(const Ptr: Pointer); inline;
    procedure Remove(const Ptr: Pointer); inline;
  end;
  PCSList = ^TCSList;

  TCSVec2 = record
  strict private
    function GetArr(const Index: Integer): Single; inline;
    procedure SetArr(const Index: Integer; const Value: Single); inline;
  public
    var x, y: Single;
    property Arr[const Index: Integer]: Single read GetArr write SetArr; default;
    procedure SetValue(const _x, _y: Single); inline;
    function Len: Single; inline;
    function LenSq: Single; inline;
    function Norm: TCSVec2; inline;
    function Perp: TCSVec2; inline;
    function Dist(const v: TCSVec2): Single; inline;
    function Angle(const v: TCSVec2): Single; inline;
    function Dot(const v: TCSVec2): Single; inline;
    function Cross(const v: TCSVec2): Single; inline;
    function Lerp(const v: TCSVec2; const s: Single): TCSVec2; inline;
    function Reflect(const v: TCSVec2): TCSVec2; inline;
    function ToStr: AnsiString; inline;
    class operator Negative(const v: TCSVec2): TCSVec2;
    class operator Add(const v1, v2: TCSVec2): TCSVec2;
    class operator Add(const v: TCSVec2; const s: Single): TCSVec2;
    class operator Add(const s: Single; const v: TCSVec2): TCSVec2;
    class operator Subtract(const v1, v2: TCSVec2): TCSVec2;
    class operator Subtract(const v: TCSVec2; const s: Single): TCSVec2;
    class operator Multiply(const v1, v2: TCSVec2): Single;
    class operator Multiply(const v: TCSVec2; const s: Single): TCSVec2;
    class operator Divide(const v: TCSVec2; const s: Single): TCSVec2;
  end;
  PCSVec2 = ^TCSVec2;
  TCSVec2Arr = array[Word] of TCSVec2;
  PCSVec2Arr = ^TCSVec2Arr;

  TCSMat2 = record
  strict private
    function GetArr(const x, y: Integer): Single; inline;
    procedure SetArr(const x, y: Integer; const Value: Single); inline;
  public
    var e00, e01: Single;
    var e10, e11: Single;
    property Arr[const x, y: Integer]: Single read GetArr write SetArr; default;
    procedure SetValue(const _e00, _e01, _e10, _e11: Single); inline;
    procedure SetIdentity; inline;
    procedure SetRotation(const r: Single); inline;
    procedure SetScaling(const sx, sy: Single); inline;
    function Det: Single; inline;
    function Transpose: TCSMat2; inline;
    function Inverse: TCSMat2; inline;
    function GetRotation: Single; inline;
    function ToStr: AnsiString; inline;
    class operator Add(const m1, m2: TCSMat2): TCSMat2;
    class operator Subtract(const m1, m2: TCSMat2): TCSMat2;
    class operator Multiply(const m1, m2: TCSMat2): TCSMat2;
    class operator Multiply(const v: TCSVec2; const m: TCSMat2): TCSVec2;
  end;
  PCSMat2 = ^TCSMat2;
  TCSMat2Arr = array[Word] of TCSMat2;
  PCSMat2Arr = ^TCSMat2Arr;
  TCSMat2Ref = array[0..1, 0..1] of Single;
  PCSMat2Ref = ^TCSMat2Ref;

  TCSMat3 = record
  strict private
    function GetArr(const x, y: Integer): Single; inline;
    procedure SetArr(const x, y: Integer; const Value: Single); inline;
  public
    var e00, e01: Single;
    var e10, e11: Single;
    var e20, e21: Single;
    property Arr[const x, y: Integer]: Single read GetArr write SetArr; default;
    procedure SetValue(const _e00, _e01, _e10, _e11, _e20, _e21: Single); inline;
    procedure SetIdentity; inline;
    procedure SetTranslation(const x, y: Single); inline;
    procedure SetRotation(const r: Single); inline;
    procedure SetScaling(const sx, sy: Single); inline;
    function Det: Single; inline;
    function Inverse: TCSMat3; inline;
    function GetRotation: Single; inline;
    function ToStr: AnsiString; inline;
    class operator Add(const m1, m2: TCSMat3): TCSMat3;
    class operator Subtract(const m1, m2: TCSMat3): TCSMat3;
    class operator Multiply(const m1, m2: TCSMat3): TCSMat3;
    class operator Multiply(const v: TCSVec2; const m: TCSMat3): TCSVec2;
  end;
  PCSMat3 = ^TCSMat3;
  TCSMat3Arr = array[Word] of TCSMat3;
  PCSMat3Arr = ^TCSMat3Arr;
  TCSMat3Ref = array[0..2, 0..1] of Single;
  PCSMat3Ref = ^TCSMat3Ref;

  TCSPoly = record
  strict private
    function GetArr(const Index: Integer): PCSVec2; inline;
  public
    var v: array of TCSVec2;
    property Arr[const Index: Integer]: PCSVec2 read GetArr; default;
    procedure SetPolygon(const n: Integer; const r: Single); inline;
    procedure Translate(const x, y: Single); overload; inline;
    procedure Translate(const Pos: TCSVec2); overload; inline;
    class operator Multiply(const p: TCSPoly; const m: TCSMat2): TCSPoly;
    class operator Multiply(const p: TCSPoly; const m: TCSMat3): TCSPoly;
  end;
  PCSPoly = ^TCSPoly;
  TCSPolyArr = array[Word] of TCSPoly;
  PCSPolyArr = ^TCSPoly;

  TCSCircle = record
  strict private
    function GetArr(const Index: Integer): Single; inline;
    procedure SetArr(const Index: Integer; const Value: Single); inline;
  public
    var c: TCSVec2;
    var r: Single;
    var a: Single;
    property Arr[const Index: Integer]: Single read GetArr write SetArr; default;
    procedure SetValue(const _c: TCSVec2; const _r, _a: Single); inline;
  end;
  PCSCircle = ^TCSCircle;
  TCSCircleArr = array[Word] of TCSCircle;
  PCSCircleArr = ^TCSCircleArr;

  TCSResp = record
  strict private
    function GetArr(const Index: Integer): Single; inline;
    procedure SetArr(const Index: Integer; const Value: Single); inline;
  public
    var n: TCSVec2;
    var d: Single;
    property Arr[const Index: Integer]: Single read GetArr write SetArr; default;
  end;
  PCSResp = ^TCSResp;
  TCSRespArr = array[Word] of TCSResp;
  PCSRespArr = ^TCSRespArr;

  TCSCont = record
  strict private
    function GetArr(const Index: Integer): Single; inline;
    procedure SetArr(const Index: Integer; const Value: Single); inline;
  public
    var p: TCSVec2;
    var n: TCSVec2;
    property Arr[const Index: Integer]: Single read GetArr write SetArr; default;
  end;
  PCSCont = ^TCSCont;

  TCSContGroup = record
  strict private
    procedure SetCapacity(const Value: Integer); inline;
    function GetCapacity: Integer; inline;
  public
    var c: array of TCSCont;
    var Count: Integer;
    property Capacity: Integer read GetCapacity write SetCapacity;
    procedure Add(const Cont: TCSCont); inline;
  end;
  PCSContGroup = ^TCSContGroup;

  TCSColy = record
  strict private
    procedure SetCapacity(const Value: Integer); inline;
    function GetCapacity: Integer; inline;
  public
    var c: array of record
      var n1, n2: TCSNode;
    end;
    var Count: Integer;
    property Capacity: Integer read GetCapacity write SetCapacity;
    procedure Add(const n1, n2: TCSNode); inline;
  end;

  TCSBounds = record
  strict private
    function GetArr(const Index: Integer): TCSVec2; inline;
    procedure SetArr(const Index: Integer; const Value: TCSVec2); inline;
  public
    var c, nx, ny: TCSVec2;
    property Arr[const Index: Integer]: TCSVec2 read GetArr write SetArr; default;
    procedure Transform(const m: TCSMat3); inline;
    function GetRect: TCSRect; inline;
  end;

  TCSGeom = class
  strict private
  strict protected
  public
    var b: TCSBounds;
    constructor Create;
    destructor Destroy; override;
    procedure Normalize; virtual; abstract;
    procedure GenerateBounds; virtual; abstract;
    function Clone: TCSGeom; virtual; abstract;
  end;

  TCSGeomCircle = class (TCSGeom)
  strict private
  strict protected
  public
    var c: TCSCircle;
    constructor Create;
    destructor Destroy; override;
    procedure Normalize; override;
    procedure GenerateBounds; override;
    function Clone: TCSGeom; override;
  end;

  TCSGeomPoly = class (TCSGeom)
  strict private
  strict protected
  public
    var p: TCSPoly;
    constructor Create;
    destructor Destroy; override;
    procedure Normalize; override;
    procedure GenerateBounds; override;
    function Clone: TCSGeom; override;
  end;

  TCSNode = class
  strict private
  strict protected
  public
    var World: TCSWorld;
    var xl, xh, yl, yh: Integer;
    var Transform: TCSMat3;
    var VelLin: TCSVec2;
    var VelAng: Single;
    var AccLin: TCSVec2;
    var AccAng: Single;
    var IsStatic: Boolean;
    var IsFrozen: Boolean;
    var FreezeTime: DWord;
    var Rect: TCSRect;
    constructor Create; virtual;
    destructor Destroy; override;
    procedure Update(const StepSize: Single); virtual;
  end;

  TCSBody = class (TCSNode)
  strict private
    procedure SetGeom(const Value: TCSGeom); inline;
  strict protected
    var m_GeomInit: TCSGeom;
    var m_Geom: TCSGeom;
  public
    var Mass: Single;
    var Hardness: Single;
    var Friction: Single;
    property Geom: TCSGeom read m_Geom write SetGeom;
    constructor Create; override;
    destructor Destroy; override;
    procedure Update(const StepSize: Single); override;
    procedure UpdateGeom;
  end;

  TCSWorld = class
  strict private
    type TGridCell = record
      var Bounds: TCSRect;
      var Nodes: TCSList;
    end;
    var m_Grid: array of array of TGridCell;
    var m_X: Integer;
    var m_Y: Integer;
    var m_Width: Integer;
    var m_Height: Integer;
    var m_CellSizeX: Integer;
    var m_CellSizeY: Integer;
    var m_Nodes: TCSList;
    var m_cg: TCSContGroup;
    var m_UpdateSteps: Integer;
    function GetNode(const Index: Integer): TCSNode;
    function GetNodeCount: Integer; inline;
  strict protected
  public
    var m_Coly: TCSColy;
    constructor Create(const _X, _Y, _Width, _Height, _GridSizeX, _GridSizeY: Integer);
    destructor Destroy; override;
    property X: Integer read m_X;
    property Y: Integer read m_Y;
    property Width: Integer read m_Width;
    property Height: Integer read m_Height;
    property CellSizeX: Integer read m_CellSizeX;
    property CellSizeY: Integer read m_CellSizeY;
    property Nodes[const Index: Integer]: TCSNode read GetNode;
    property NodeCount: Integer read GetNodeCount;
    procedure NodeAdd(const n: TCSNode); inline;
    procedure NodeRemove(const n: TCSNode); inline;
    procedure Update;
    function Collide(const b1, b2: TCSBody): Boolean; inline;
  end;

  procedure CSSinCos(const Angle: Single; var s, c: Single); assembler;
  function CSArcTan2(const y, x: Single): Single; assembler;
  function CSSign(const f: Single): Single; inline;

  function CSRect(const l, t, r, b: Integer): TCSRect; inline;

  function CSVec2(const x, y: Single): TCSVec2; inline;
  function CSVec2Len(const v: TCSVec2): Single; inline;
  function CSVec2LenSq(const v: TCSVec2): Single; inline;
  function CSVec2Norm(const v: TCSVec2): TCSVec2; assembler;
  function CSVec2Add(const v1, v2: TCSVec2): TCSVec2; inline;
  function CSVec2Sub(const v1, v2: TCSVec2): TCSVec2; inline;
  function CSVec2Scale(const v: TCSVec2; const s: Single): TCSVec2; inline;
  function CSVec2Dot(const v1, v2: TCSVec2): Single; inline;
  function CSVec2Cross(const v1, v2: TCSVec2): Single; inline;
  function CSVec2Dist(const v1, v2: TCSVec2): Single; assembler;
  function CSVec2DistSq(const v1, v2: TCSVec2): Single; inline;
  function CSVec2Angle(const v1, v2: TCSVec2): Single; assembler;
  function CSVec2Perp(const v: TCSVec2): TCSVec2; inline;
  function CSVec2Reflect(const v, n: TCSVec2): TCSVec2; inline;
  function CSVec2Lerp(const v1, v2: TCSVec2; const s: Single): TCSVec2; inline;
  function CSVec2Mat2Mul(const v: TCSVec2; const m: TCSMat2): TCSVec2; inline;
  function CSVec2Mat3Mul(const v: TCSVec2; const m: TCSMat3): TCSVec2; inline;
  function CSVec2ToStr(const v: TCSVec2): AnsiString;

  function CSMat2(const e00, e01, e10, e11: Single): TCSMat2; inline;
  function CSMat2Identity: TCSMat2; inline;
  function CSMat2Scaling(const sx, sy: Single): TCSMat2; inline;
  function CSMat2Rotation(const r: Single): TCSMat2; inline;
  function CSMat2Det(const m: TCSMat2): Single; inline;
  function CSMat2Add(const m1, m2: TCSMat2): TCSMat2; inline;
  function CSMat2Sub(const m1, m2: TCSMat2): TCSMat2; inline;
  function CSMat2Scale(const m: TCSMat2; s: Single): TCSMat2; inline;
  function CSMat2Mul(const m1, m2: TCSMat2): TCSMat2; assembler;
  function CSMat2Transpose(const m: TCSMat2): TCSMat2; inline;
  function CSMat2Inverse(const m: TCSMat2): TCSMat2; inline;
  function CSMat2ExtRotation(const m: TCSMat2): Single; inline;
  function CSMat2ToStr(const m: TCSMat2): AnsiString;

  function CSMat3(const e00, e01, e10, e11, e20, e21: Single): TCSMat3; inline;
  function CSMat3Identity: TCSMat3; inline;
  function CSMat3Translation(const x, y: Single): TCSMat3; inline;
  function CSMat3Scaling(const sx, sy: Single): TCSMat3; inline;
  function CSMat3Rotation(const r: Single): TCSMat3; inline;
  function CSMat3Det(const m: TCSMat3): Single; inline;
  function CSMat3Add(const m1, m2: TCSMat3): TCSMat3; inline;
  function CSMat3Sub(const m1, m2: TCSMat3): TCSMat3; inline;
  function CSMat3Scale(const m: TCSMat3; s: Single): TCSMat3; inline;
  function CSMat3Mul(const m1, m2: TCSMat3): TCSMat3; assembler;
  function CSMat3Inverse(const m: TCSMat3): TCSMat3; inline;
  function CSMat3ExtRotation(const m: TCSMat3): Single;
  function CSMat3ToStr(const m: TCSMat3): AnsiString;

  function CSPoly(const n: Integer; const r: Single): TCSPoly; inline;
  procedure CSPolyEdge(const p: TCSPoly; const EdgeIndex: Integer; var Index1, Index2: Integer); inline;
  function CSPolyEdgeDir(const p: TCSPoly; const EdgeIndex: Integer): TCSVec2; inline;
  function CSPolyEdgeNormal(const p: TCSPoly; const EdgeIndex: Integer): TCSVec2; inline;
  function CSPolyCenter(const p: TCSPoly): TCSVec2; inline;
  function CSPolyBounds(const p: TCSPoly): TCSBounds; inline;
  procedure CSPolyMove(const p: TCSPoly; const Pos: TCSVec2); inline;
  function CSPolyTransform(const p: TCSPoly; const m: TCSMat2): TCSPoly; inline; overload;
  function CSPolyTransform(const p: TCSPoly; const m: TCSMat3): TCSPoly; inline; overload;

  function CSCircle(const c: TCSVec2; const r, a: Single): TCSCircle; inline;
  function CSResp(const n: TCSVec2; const d: Single): TCSResp; inline;
  function CSCont(const p: TCSVec2; const n: TCSVec2): TCSCont; inline;
  function CSBounds(const c, nx, ny: TCSVec2): TCSBounds; inline;
  function CSBoundsMat3Mul(const b: TCSBounds; const m: TCSMat3): TCSBounds; inline;

  procedure CSPolyProject(const a: TCSVec2; const p: TCSPoly; var Min, Max: Single); inline;
  procedure CSCircleProject(const a: TCSVec2; const c: TCSCircle; var Min, Max: Single); inline;
  function CSAxisSeparatePoly(const a: TCSVec2; const p1, p2: TCSPoly; const r: PCSResp = nil): Boolean; //inline;
  function CSAxisSeparateCircle(const a: TCSVec2; const c1, c2: TCSCircle; const r: PCSResp = nil): Boolean; inline;
  function CSAxisSeparatePolyCircle(const a: TCSVec2; const p: TCSPoly; const c: TCSCircle; const r: PCSResp = nil): Boolean; inline;
  function CSAxisSeparateCirclePoly(const a: TCSVec2; const c: TCSCircle; const p: TCSPoly; const r: PCSResp = nil): Boolean; inline;
  function CSIntersectPoly(const p1, p2: TCSPoly; const r: PCSResp = nil): Boolean;
  function CSIntersectCircle(const c1, c2: TCSCircle; const r: PCSResp = nil): Boolean;
  function CSIntersectPolyCircle(const p: TCSPoly; const c: TCSCircle; const r: PCSResp = nil): Boolean;
  function CSIntersectCirclePoly(const c: TCSCircle; const p: TCSPoly; const r: PCSResp = nil): Boolean;
  procedure CSFindSupportPointsPoly(const n: TCSVec2; const p: TCSPoly; const List: PCSList); inline;
  procedure CSFindContactsPoly(const n: TCSVec2; const p1, p2: TCSPoly; const Cont: PCSContGroup);
  procedure CSFindContactsCircle(const n: TCSVec2; const c1, c2: TCSCircle; const Cont: PCSContGroup); inline;
  procedure CSFindContactsPolyCircle(const n: TCSVec2; const p: TCSPoly; const c: TCSCircle; const Cont: PCSContGroup); inline;
  procedure CSFindContactsCirclePoly(const n: TCSVec2; const c: TCSCircle; const p: TCSPoly; const Cont: PCSContGroup); inline;
  function CSFindMTD(const RCount: Integer): TCSResp;
  function CSGetForceAtPoint(const Body: TCSBody; const p: TCSVec2): TCSVec2; inline;
  procedure CSSetForceAtPoint(const Body: TCSBody; const p, f: TCSVec2); inline;
  procedure CSResolveIntersection(const Body1, Body2: TCSBody; const r: TCSResp); inline;
  function CSResolveForces(
    const N1, N2, F1, F2: TCSVec2;
    var Direct1: TCSVec2;
    var Friction1: TCSVec2;
    var Direct2: TCSVec2;
    var Friction2: TCSVec2;
    const MagHardness: Single = 1;
    const MagFriction: Single = 1
  ): Boolean; inline;
  procedure CSResolveContacts(const Body1, Body2: TCSBody; const Contacts: TCSContGroup; const r: TCSResp); inline;

const
  CSQuatPi = 0.25 * pi;
  CSHalfPi = 0.5 * pi;
  CSTwoPi = 2 * pi;

implementation

var
  g_RespArr: array of TCSResp;
  g_sp1, g_sp2, g_sp3: TCSList;

//TCSRect BEGIN
procedure TCSRect.SetValue(const _l, _t, _r, _b: Integer);
begin
  l := _l; t := _t; r := _r; b := _b;
end;

function TCSRect.Width: Integer;
begin
  Result := r - l;
end;

function TCSRect.Height: Integer;
begin
  Result := b - t;
end;

function TCSRect.Intersect(const r: TCSRect): Boolean;
begin
  Result := (Self.l <= r.r) and (Self.t <= r.b) and (Self.r >= r.l) and (Self.b >= r.t);
end;
//TCSRect END

//TCSList BEGIN
procedure TCSList.SetElement(const Index: Integer; const Value: Pointer);
begin
  m_Arr[Index] := Value;
end;

function TCSList.GetElement(const Index: Integer): Pointer;
begin
  Result := m_Arr[Index];
end;

procedure TCSList.SetCapacity(const Value: Integer);
begin
  SetLength(m_Arr, Value);
end;

function TCSList.GetCapacity: Integer;
begin
  Result := Length(m_Arr);
end;

procedure TCSList.Add(const Ptr: Pointer);
begin
  if m_Count >= Length(m_Arr) then
  SetLength(m_Arr, Length(m_Arr) + 32);
  m_Arr[m_Count] := Ptr;
  Inc(m_Count);
end;

procedure TCSList.Remove(const Ptr: Pointer);
var
  i: Integer;
begin
  for i := m_Count - 1 downto 0 do
  if m_Arr[i] = Ptr then
  begin
    Move(m_Arr[i + 1], m_Arr[i], (m_Count - (i + 1)) * 4);
    Dec(m_Count);
    Exit;
  end;
end;
//TCSList END

//TCSVec2 BEGIN
function TCSVec2.GetArr(const Index: Integer): Single;
begin
  Result := PFloatArray(@x)^[Index];
end;

procedure TCSVec2.SetArr(const Index: Integer; const Value: Single);
begin
  PFloatArray(@x)^[Index] := Value;
end;

procedure TCSVec2.SetValue(const _x, _y: Single);
begin
  x := _x;
  y := _y;
end;

function TCSVec2.Len: Single;
begin
  Result := CSVec2Len(Self);
end;

function TCSVec2.LenSq: Single;
begin
  Result := CSVec2LenSq(Self);
end;

function TCSVec2.Norm: TCSVec2;
begin
  Result := CSVec2Norm(Self);
end;

function TCSVec2.Perp: TCSVec2;
begin
  Result := CSVec2Perp(Self);
end;

function TCSVec2.Dist(const v: TCSVec2): Single;
begin
  Result := CSVec2Dist(Self, v);
end;

function TCSVec2.Angle(const v: TCSVec2): Single;
begin
  Result := CSVec2Angle(Self, v);
end;

function TCSVec2.Dot(const v: TCSVec2): Single;
begin
  Result := CSVec2Dot(Self, v);
end;

function TCSVec2.Cross(const v: TCSVec2): Single;
begin
  Result := CSVec2Cross(Self, v);
end;

function TCSVec2.Lerp(const v: TCSVec2; const s: Single): TCSVec2;
begin
  Result := CSVec2Lerp(Self, v, s);
end;

function TCSVec2.Reflect(const v: TCSVec2): TCSVec2;
begin
  Result := CSVec2Reflect(Self, v);
end;

function TCSVec2.ToStr: AnsiString;
begin
  Result := CSVec2ToStr(Self);
end;

class operator TCSVec2.Negative(const v: TCSVec2): TCSVec2;
begin
  Result.x := -v.x;
  Result.y := -v.y;
end;

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

class operator TCSVec2.Add(const v: TCSVec2; const s: Single): TCSVec2;
begin
  Result.x := v.x + s;
  Result.y := v.y + s;
end;

class operator TCSVec2.Add(const s: Single; const v: TCSVec2): TCSVec2;
begin
  Result.x := v.x + s;
  Result.y := v.y + s;
end;

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

class operator TCSVec2.Subtract(const v: TCSVec2; const s: Single): TCSVec2;
begin
  Result.x := v.x - s;
  Result.y := v.y - s;
end;

class operator TCSVec2.Multiply(const v1, v2: TCSVec2): Single;
begin
  Result := CSVec2Dot(v1, v2);
end;

class operator TCSVec2.Multiply(const v: TCSVec2; const s: Single): TCSVec2;
begin
  Result := CSVec2Scale(v, s);
end;

class operator TCSVec2.Divide(const v: TCSVec2; const s: Single): TCSVec2;
var
  rcp: Single;
begin
  rcp := 1 / s;
  Result := CSVec2Scale(v, rcp);
end;
//TCSVec2 END

//TCSMat2 BEGIN
function TCSMat2.GetArr(const x, y: Integer): Single;
begin
  Result := PCSMat2Ref(@e00)^[x, y];
end;

procedure TCSMat2.SetArr(const x, y: Integer; const Value: Single);
begin
  PCSMat2Ref(@e00)^[x, y] := Value;
end;

procedure TCSMat2.SetValue(const _e00, _e01, _e10, _e11: Single);
begin
  e00 := _e00; e01 := _e01; e10 := _e10; e11 := _e11;
end;

procedure TCSMat2.SetIdentity;
begin
  Self := CSMat2Identity;
end;

procedure TCSMat2.SetRotation(const r: Single);
begin
  Self := CSMat2Rotation(r);
end;

procedure TCSMat2.SetScaling(const sx, sy: Single);
begin
  Self := CSMat2Scaling(sx, sy);
end;

function TCSMat2.Det: Single;
begin
  Result := CSMat2Det(Self);
end;

function TCSMat2.Transpose: TCSMat2;
begin
  Result := CSMat2Transpose(Self);
end;

function TCSMat2.Inverse: TCSMat2;
begin
  Result := CSMat2Inverse(Self);
end;

function TCSMat2.GetRotation: Single;
begin
  Result := CSMat2ExtRotation(Self);
end;

function TCSMat2.ToStr: AnsiString;
begin
  Result := CSMat2ToStr(Self);
end;

class operator TCSMat2.Add(const m1, m2: TCSMat2): TCSMat2;
begin
  Result := CSMat2Add(m1, m2);
end;

class operator TCSMat2.Subtract(const m1, m2: TCSMat2): TCSMat2;
begin
  Result := CSMat2Sub(m1, m2);
end;

class operator TCSMat2.Multiply(const m1, m2: TCSMat2): TCSMat2;
begin
  Result := CSMat2Mul(m1, m2);
end;

class operator TCSMat2.Multiply(const v: TCSVec2; const m: TCSMat2): TCSVec2;
begin
  Result := CSVec2Mat2Mul(v, m);
end;
//TCSMat2 END

//TCSMat3 BEGIN
function TCSMat3.GetArr(const x, y: Integer): Single;
begin
  Result := PCSMat3Ref(@e00)^[x, y];
end;

procedure TCSMat3.SetArr(const x, y: Integer; const Value: Single);
begin
  PCSMat3Ref(@e00)^[x, y] := Value;
end;

procedure TCSMat3.SetValue(const _e00, _e01, _e10, _e11, _e20, _e21: Single);
begin
  e00 := _e00; e01 := _e01; e10 := _e10; e11 := _e11; e20 := _e20; e21 := _e21;
end;

procedure TCSMat3.SetIdentity;
begin
  Self := CSMat3Identity;
end;

procedure TCSMat3.SetTranslation(const x, y: Single);
begin
  Self := CSMat3Translation(x, y);
end;

procedure TCSMat3.SetRotation(const r: Single);
begin
  Self := CSMat3Rotation(r);
end;

procedure TCSMat3.SetScaling(const sx, sy: Single);
begin
  Self := CSMat3Scaling(sx, sy);
end;

function TCSMat3.Det: Single;
begin
  Result := CSMat3Det(Self);
end;

function TCSMat3.Inverse: TCSMat3;
begin
  Result := CSMat3Inverse(Self);
end;

function TCSMat3.GetRotation: Single;
begin
  Result := CSMat3ExtRotation(Self);
end;

function TCSMat3.ToStr: AnsiString;
begin
  Result := CSMat3ToStr(Self);
end;

class operator TCSMat3.Add(const m1, m2: TCSMat3): TCSMat3;
begin
  Result := CSMat3Add(m1, m2);
end;

class operator TCSMat3.Subtract(const m1, m2: TCSMat3): TCSMat3;
begin
  Result := CSMat3Sub(m1, m2);
end;

class operator TCSMat3.Multiply(const m1, m2: TCSMat3): TCSMat3;
begin
  Result := CSMat3Mul(m1, m2);
end;

class operator TCSMat3.Multiply(const v: TCSVec2; const m: TCSMat3): TCSVec2;
begin
  Result := CSVec2Mat3Mul(v, m);
end;
//TCSMat3 END

//TCSPoly BEGIN
function TCSPoly.GetArr(const Index: Integer): PCSVec2;
begin
  Result := @v[Index];
end;

procedure TCSPoly.SetPolygon(const n: Integer; const r: Single);
begin
  Self := CSPoly(n, r);
end;

procedure TCSPoly.Translate(const x, y: Single);
begin
  CSPolyMove(Self, CSVec2(x, y));
end;

procedure TCSPoly.Translate(const Pos: TCSVec2);
begin
  CSPolyMove(Self, Pos);
end;

class operator TCSPoly.Multiply(const p: TCSPoly; const m: TCSMat2): TCSPoly;
var
  i: Integer;
begin
  for i := 0 to High(p.v) do
  Result.v[i] := CSVec2Mat2Mul(p.v[i], m);
end;

class operator TCSPoly.Multiply(const p: TCSPoly; const m: TCSMat3): TCSPoly;
var
  i: Integer;
begin
  for i := 0 to High(p.v) do
  Result.v[i] := CSVec2Mat3Mul(p.v[i], m);
end;
//TCSPoly END

//TCSCircle BEGIN
function TCSCircle.GetArr(const Index: Integer): Single;
begin
  Result := PFloatArray(@c.x)^[Index];
end;

procedure TCSCircle.SetArr(const Index: Integer; const Value: Single);
begin
  PFloatArray(@c.x)^[Index] := Value;
end;

procedure TCSCircle.SetValue(const _c: TCSVec2; const _r, _a: Single);
begin
  c := _c; r := _r; a := _a;
end;
//TCSCircle END

//TCSResp BEGIN
function TCSResp.GetArr(const Index: Integer): Single;
begin
  Result := PFloatArray(@n.x)^[Index];
end;

procedure TCSResp.SetArr(const Index: Integer; const Value: Single);
begin
  PFloatArray(@n.x)^[Index] := Value;
end;
//TCSResp END

//TCSCont BEGIN
function TCSCont.GetArr(const Index: Integer): Single;
begin
  Result := PFloatArray(@p.x)^[Index];
end;

procedure TCSCont.SetArr(const Index: Integer; const Value: Single);
begin
  PFloatArray(@p.x)^[Index] := Value;
end;
//TCSCont END

//TCSContGroup BEGIN
procedure TCSContGroup.SetCapacity(const Value: Integer);
begin
  SetLength(c, Value);
end;

function TCSContGroup.GetCapacity: Integer;
begin
  Result := Length(c);
end;

procedure TCSContGroup.Add(const Cont: TCSCont);
begin
  if Count >= Length(c) then
  SetLength(c, Length(c) + 32);
  c[Count] := Cont;
  Inc(Count);
end;
//TCSContGroup END

//TCSColy BEGIN
procedure TCSColy.SetCapacity(const Value: Integer);
begin
  SetLength(c, Value);
end;

function TCSColy.GetCapacity: Integer;
begin
  Result := Length(c);
end;

procedure TCSColy.Add(const n1, n2: TCSNode);
var
  i: Integer;
begin
  for i := 0 to Count - 1 do
  if ((c[i].n1 = n1) and (c[i].n2 = n2)) or ((c[i].n1 = n2) and (c[i].n2 = n1)) then Exit;
  if Count >= Length(c) then
  SetLength(c, Length(c) + 32);
  c[Count].n1 := n1;
  c[Count].n2 := n2;
  Inc(Count);
end;
//TCSColy END

//TCSBounds BEGIN
function TCSBounds.GetArr(const Index: Integer): TCSVec2;
begin
  Result := PCSVec2Arr(@c)^[Index];
end;

procedure TCSBounds.SetArr(const Index: Integer; const Value: TCSVec2);
begin
  PCSVec2Arr(@c)^[Index] := Value;
end;

procedure TCSBounds.Transform(const m: TCSMat3);
begin
  Self := CSBoundsMat3Mul(Self, m);
end;

function TCSBounds.GetRect: TCSRect;
var
  pts: array[0..3] of TCSVec2;
  MinV, MaxV: TCSVec2;
  i: Integer;
begin
  pts[0] := CSVec2Add(nx, ny);
  pts[1] := CSVec2Add(-nx, ny);
  pts[2] := CSVec2Add(nx, -ny);
  pts[3] := CSVec2Add(-nx, -ny);
  MinV := pts[0];
  MaxV := MinV;
  for i := 1 to 3 do
  begin
    if pts[i].x < MinV.x then MinV.x := pts[i].x;
    if pts[i].x > MaxV.x then MaxV.x := pts[i].x;
    if pts[i].y < MinV.y then MinV.y := pts[i].y;
    if pts[i].y > MaxV.y then MaxV.y := pts[i].y;
  end;
  MinV := CSVec2Add(MinV, c);
  MaxV := CSVec2Add(MaxV, c);
  Result.l := Trunc(MinV.x);
  Result.t := Trunc(MinV.y);
  Result.r := Trunc(MaxV.x) + 1;
  Result.b := Trunc(MaxV.y) + 1;
end;
//TCSBounds END

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

destructor TCSGeom.Destroy;
begin
  inherited Destroy;
end;
//TCSGeom END

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

destructor TCSGeomCircle.Destroy;
begin
  inherited Destroy;
end;

procedure TCSGeomCircle.Normalize;
begin
  c.c.SetValue(0, 0);
end;

procedure TCSGeomCircle.GenerateBounds;
begin
  b.c := c.c;
  b.nx.SetValue(c.r, 0);
  b.ny.SetValue(0, c.r);
end;

function TCSGeomCircle.Clone: TCSGeom;
begin
  Result := TCSGeomCircle.Create;
  Result.b := b;
  TCSGeomCircle(Result).c := c;
end;
//TCSGeomCircle END

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

destructor TCSGeomPoly.Destroy;
begin
  inherited Destroy;
end;

procedure TCSGeomPoly.Normalize;
begin
  CSPolyMove(p, -CSPolyCenter(p));
end;

procedure TCSGeomPoly.GenerateBounds;
begin
  b := CSPolyBounds(p);
end;

function TCSGeomPoly.Clone: TCSGeom;
begin
  Result := TCSGeomPoly.Create;
  Result.b := b;
  SetLength(TCSGeomPoly(Result).p.v, Length(p.v));
  Move(p.v[0], TCSGeomPoly(Result).p.v[0], SizeOf(TCSVec2) * Length(p.v));
end;
//TCSGeomPoly END

//TCSNode BEGIN
constructor TCSNode.Create;
begin
  inherited Create;
  Transform.SetIdentity;
  VelLin.SetValue(0, 0);
  VelAng := 0;
  AccLin.SetValue(0, 0);
  AccAng := 0;
  IsStatic := False;
  IsFrozen := False;
  FreezeTime := GetTickCount;
  xl := 0;
  xh := 0;
  yl := 0;
  yh := 0;
end;

destructor TCSNode.Destroy;
begin
  inherited Destroy;
end;

procedure TCSNode.Update(const StepSize: Single);
const
  MAX_VEL_LIN = 16;
  MAX_VEL_LIN_SQ = MAX_VEL_LIN * MAX_VEL_LIN;
  MAX_VEL_ANG = Pi * 6E-2;
  FREEZE_VEL_LIN = 0.5;
  FREEZE_VEL_ANG = Pi * 8E-4;
  UNFREEZE_VEL_LIN = 1;
  UNFREEZE_VEL_ANG = Pi * 8E-4;
  FREEZE_TIME = 5000;
var
  m: TCSMat2;
begin
  if not IsStatic then
  begin
    if IsFrozen then
    begin
      if (VelLin.LenSq > UNFREEZE_VEL_LIN) or (VelAng > UNFREEZE_VEL_ANG) then
      begin
        IsFrozen := False;
        FreezeTime := GetTickCount;
      end;
    end
    else
    begin
      if (VelLin.LenSq > FREEZE_VEL_LIN) or (VelAng > FREEZE_VEL_ANG) then
      FreezeTime := GetTickCount;
      if GetTickCount - FreezeTime > FREEZE_TIME then
      IsFrozen := True;
    end;
    if not IsFrozen then
    begin
      VelLin := CSVec2Add(VelLin, AccLin);
      AccLin.SetValue(0, 0);
      VelAng := VelAng + AccAng;
      AccAng := 0;
      if VelLin.LenSq > MAX_VEL_LIN_SQ then
      VelLin := CSVec2Scale(VelLin.Norm, MAX_VEL_LIN);
      if Abs(VelAng) > MAX_VEL_ANG then
      VelAng := MAX_VEL_ANG * CSSign(VelAng);
      m.SetRotation(VelAng * StepSize);
      PCSMat2(@Transform)^ := CSMat2Mul(PCSMat2(@Transform)^, m);
      Transform.e20 := Transform.e20 + VelLin.x * StepSize;
      Transform.e21 := Transform.e21 + VelLin.y * StepSize;
    end;
  end;
end;
//TCSNode END

//TCSBody BEGIN
procedure TCSBody.SetGeom(const Value: TCSGeom);
begin
  m_GeomInit := Value.Clone;
  Transform := CSMat3Identity;
  if Assigned(m_Geom) then
  begin
    m_Geom.Free;
    m_Geom := nil;
  end;
  if Assigned(m_GeomInit) then
  begin
    m_GeomInit.GenerateBounds;
    m_Geom := m_GeomInit.Clone;
  end;
end;

constructor TCSBody.Create;
begin
  inherited Create;
  m_Geom := nil;
  Mass := 1;
  Hardness := 0.9;
  Friction := 0.5;
end;

destructor TCSBody.Destroy;
begin
  if Assigned(m_Geom) then
  begin
    m_Geom.Free;
    m_Geom := nil;
  end;
  if Assigned(m_GeomInit) then
  begin
    m_GeomInit.Free;
    m_GeomInit := nil;
  end;
  inherited Destroy;
end;

procedure TCSBody.Update(const StepSize: Single);
begin
  inherited Update(StepSize);
  UpdateGeom;
end;

procedure TCSBody.UpdateGeom;
var
  i: Integer;
begin
  m_Geom.b := CSBoundsMat3Mul(m_GeomInit.b, Transform);
  if m_Geom is TCSGeomPoly then
  for i := 0 to High(TCSGeomPoly(m_Geom).p.v) do
  TCSGeomPoly(m_Geom).p.v[i] := CSVec2Mat3Mul(TCSGeomPoly(m_GeomInit).p.v[i], Transform)
  else if m_Geom is TCSGeomCircle then
  begin
    TCSGeomCircle(m_Geom).c.a := TCSGeomCircle(m_GeomInit).c.a + Transform.GetRotation;
    TCSGeomCircle(m_Geom).c.c := TCSGeomCircle(m_GeomInit).c.c + PCSVec2(@Transform.e20)^;
  end;
  Rect := m_Geom.b.GetRect;
end;
//TCSBody END

//TCSWorld BEGIN
function TCSWorld.GetNode(const Index: Integer): TCSNode;
begin
  Result := TCSNode(m_Nodes[Index]);
end;

function TCSWorld.GetNodeCount: Integer;
begin
  Result := m_Nodes.Count;
end;

constructor TCSWorld.Create(const _X, _Y, _Width, _Height, _GridSizeX, _GridSizeY: Integer);
var
  i, j: Integer;
begin
  inherited Create;
  m_X := _X;
  m_Y := _Y;
  m_Width := _Width;
  m_Height := _Height;
  m_UpdateSteps := 4;
  m_Nodes.Capacity := 16;
  m_Nodes.Count := 0;
  m_cg.Capacity := 16;
  m_cg.Count := 0;
  m_Coly.Capacity := 16;
  m_Coly.Count := 0;
  SetLength(m_Grid, _GridSizeX, _GridSizeY);
  m_CellSizeX := m_Width div _GridSizeX;
  m_CellSizeY := m_Height div _GridSizeY;
  for j := 0 to High(m_Grid[0]) do
  for i := 0 to High(m_Grid) do
  begin
    m_Grid[i, j].Nodes.Capacity := 16;
    m_Grid[i, j].Nodes.Count := 0;
    m_Grid[i, j].Bounds.SetValue(i * m_CellSizeX, j * m_CellSizeY, (i + 1) * m_CellSizeX, (j + 1) * m_CellSizeY);
  end;
end;

destructor TCSWorld.Destroy;
var
  i: Integer;
begin
  for i := 0 to m_Nodes.Count - 1 do
  TCSNode(m_Nodes[i]).Free;
  m_Nodes.Count := 0;
  inherited Destroy;
end;

procedure TCSWorld.NodeAdd(const n: TCSNode);
var
  i, j: Integer;
begin
  n.World := Self;
  m_Nodes.Add(n);
  n.xl := (n.Rect.l - m_X) div m_CellSizeX;
  n.xh := (n.Rect.r - m_X) div m_CellSizeX;
  n.yl := (n.Rect.t - m_Y) div m_CellSizeY;
  n.yh := (n.Rect.b - m_Y) div m_CellSizeY;
  for j := n.yl to n.yh do
  for i := n.xl to n.xh do
  if (i >= 0) and (j >= 0) and (i <= High(m_Grid)) and (j <= High(m_Grid[0])) then
  m_Grid[i, j].Nodes.Add(n);
end;

procedure TCSWorld.NodeRemove(const n: TCSNode);
var
  i, j: Integer;
begin
  for j := n.yl to n.yh do
  for i := n.xl to n.xh do
  if (i >= 0) and (j >= 0) and (i <= High(m_Grid)) and (j <= High(m_Grid[0])) then
  m_Grid[i, j].Nodes.Remove(n);
  m_Nodes.Remove(n);
  if n.World = Self then
  n.World := nil;
end;

procedure TCSWorld.Update;
var
  i, j, n, n1, u, nxl, nxh, nyl, nyh: Integer;
  StepSize: Single;
begin
  StepSize := 1 / m_UpdateSteps;
  for u := 0 to m_UpdateSteps - 1 do
  begin
    m_Coly.Count := 0;
    for n := 0 to m_Nodes.Count - 1 do
    begin
      TCSNode(m_Nodes[n]).Update(StepSize);
      if not TCSNode(m_Nodes[n]).IsFrozen then
      begin
        nxl := (TCSNode(m_Nodes[n]).Rect.l - m_X) div m_CellSizeX;
        nxh := (TCSNode(m_Nodes[n]).Rect.r - m_X) div m_CellSizeX;
        nyl := (TCSNode(m_Nodes[n]).Rect.t - m_Y) div m_CellSizeY;
        nyh := (TCSNode(m_Nodes[n]).Rect.b - m_Y) div m_CellSizeY;
        if (nxl <> TCSNode(m_Nodes[n]).xl)
        or (nxh <> TCSNode(m_Nodes[n]).xh)
        or (nyl <> TCSNode(m_Nodes[n]).yl)
        or (nyh <> TCSNode(m_Nodes[n]).yh) then
        begin
          for j := TCSNode(m_Nodes[n]).yl to TCSNode(m_Nodes[n]).yh do
          for i := TCSNode(m_Nodes[n]).xl to TCSNode(m_Nodes[n]).xh do
          if (i >= 0) and (j >= 0) and (i <= High(m_Grid)) and (j <= High(m_Grid[0])) then
          m_Grid[i, j].Nodes.Remove(m_Nodes[n]);
          TCSNode(m_Nodes[n]).xl := nxl;
          TCSNode(m_Nodes[n]).xh := nxh;
          TCSNode(m_Nodes[n]).yl := nyl;
          TCSNode(m_Nodes[n]).yh := nyh;
          for j := TCSNode(m_Nodes[n]).yl to TCSNode(m_Nodes[n]).yh do
          for i := TCSNode(m_Nodes[n]).xl to TCSNode(m_Nodes[n]).xh do
          if (i >= 0) and (j >= 0) and (i <= High(m_Grid)) and (j <= High(m_Grid[0])) then
          m_Grid[i, j].Nodes.Add(m_Nodes[n]);
        end;
      end;
    end;
    for n := 0 to m_Nodes.Count - 1 do
    if not TCSNode(m_Nodes[n]).IsStatic then
    begin
      for j := TCSNode(m_Nodes[n]).yl to TCSNode(m_Nodes[n]).yh do
      for i := TCSNode(m_Nodes[n]).xl to TCSNode(m_Nodes[n]).xh do
      if (i >= 0) and (j >= 0) and (i <= High(m_Grid)) and (j <= High(m_Grid[0])) then
      for n1 := 0 to m_Grid[i, j].Nodes.Count - 1 do
      if (m_Nodes[n] <> m_Grid[i, j].Nodes[n1])
      and (TCSNode(m_Nodes[n]).Rect.Intersect(TCSNode(m_Grid[i, j].Nodes[n1]).Rect)) then
      m_Coly.Add(TCSNode(m_Nodes[n]), TCSNode(m_Grid[i, j].Nodes[n1]));
    end;
    for i := 0 to m_Coly.Count - 1 do
    if (m_Coly.c[i].n1 is TCSBody) and (m_Coly.c[i].n2 is TCSBody) then
    Collide(TCSBody(m_Coly.c[i].n1), TCSBody(m_Coly.c[i].n2));
  end;
end;

function TCSWorld.Collide(const b1, b2: TCSBody): Boolean;
var
  r: TCSResp;
begin
  Result := False;
  m_cg.Count := 0;
  if (b1.Geom is TCSGeomPoly)
  and (b2.Geom is TCSGeomPoly)
  and CSIntersectPoly(TCSGeomPoly(b1.Geom).p, TCSGeomPoly(b2.Geom).p, @r) then
  begin
    CSResolveIntersection(b1, b2, r);
    b1.UpdateGeom;
    b2.UpdateGeom;
    CSFindContactsPoly(r.n, TCSGeomPoly(b1.Geom).p, TCSGeomPoly(b2.Geom).p, @m_cg);
    CSResolveContacts(b1, b2, m_cg, r);
    Result := True;
  end
  else if (b1.Geom is TCSGeomPoly)
  and (b2.Geom is TCSGeomCircle)
  and CSIntersectPolyCircle(TCSGeomPoly(b1.Geom).p, TCSGeomCircle(b2.Geom).c, @r) then
  begin
    CSResolveIntersection(b1, b2, r);
    b1.UpdateGeom;
    b2.UpdateGeom;
    CSFindContactsPolyCircle(r.n, TCSGeomPoly(b1.Geom).p, TCSGeomCircle(b2.Geom).c, @m_cg);
    CSResolveContacts(b1, b2, m_cg, r);
    Result := True;
  end
  else if (b1.Geom is TCSGeomCircle)
  and (b2.Geom is TCSGeomPoly)
  and CSIntersectCirclePoly(TCSGeomCircle(b1.Geom).c, TCSGeomPoly(b2.Geom).p, @r) then
  begin
    CSResolveIntersection(b1, b2, r);
    b1.UpdateGeom;
    b2.UpdateGeom;
    CSFindContactsCirclePoly(r.n, TCSGeomCircle(b1.Geom).c, TCSGeomPoly(b2.Geom).p, @m_cg);
    CSResolveContacts(b1, b2, m_cg, r);
    Result := True;
  end
  else if (b1.Geom is TCSGeomCircle)
  and (b2.Geom is TCSGeomCircle)
  and CSIntersectCircle(TCSGeomCircle(b1.Geom).c, TCSGeomCircle(b2.Geom).c, @r) then
  begin
    CSResolveIntersection(b1, b2, r);
    b1.UpdateGeom;
    b2.UpdateGeom;
    CSFindContactsCircle(r.n, TCSGeomCircle(b1.Geom).c, TCSGeomCircle(b2.Geom).c, @m_cg);
    CSResolveContacts(b1, b2, m_cg, r);
    Result := True;
  end;
end;
//TCSWorld END

procedure CSSinCos(const Angle: Single; var s, c: Single);
asm
  fld Angle
  fsincos
  fstp [edx]
  fstp [eax]
  fwait
end;

function CSArcTan2(const y, x: Single): Single;
asm
  fld y
  fld x
  fpatan
  fwait
end;

function CSSign(const f: Single): Single;
begin
  if f >= 0 then
  Result := 1
  else
  Result := -1;
end;

function CSRect(const l, t, r, b: Integer): TCSRect;
begin
  Result.l := l;
  Result.t := t;
  Result.r := r;
  Result.b := b;
end;

function CSVec2(const x, y: Single): TCSVec2;
begin
  Result.x := x;
  Result.y := y;
end;

function CSVec2Len(const v: TCSVec2): Single;
begin
  Result := Sqrt((v.x * v.x) + (v.y * v.y));
end;

function CSVec2LenSq(const v: TCSVec2): Single;
begin
  Result := (v.x * v.x) + (v.y * v.y);
end;

function CSVec2Norm(const v: TCSVec2): TCSVec2;
asm
  movlps xmm0, [eax]
  movaps xmm1, xmm0
  mulps xmm1, xmm1
  unpcklps xmm1, xmm1
  movhlps xmm2, xmm1
  addps xmm1, xmm2
  movss [Result], xmm1
  cmp [Result], 0
  jbe @Bail
  rsqrtss xmm1, xmm1
  shufps xmm1, xmm1, 0
  mulps xmm0, xmm1
  movlps [Result], xmm0
  ret
@Bail:
  mov [Result], 0
  mov [Result + 4], 0
end;

function CSVec2Add(const v1, v2: TCSVec2): TCSVec2;
begin
  Result.x := v1.x + v2.x;
  Result.y := v1.y + v2.y;
end;

function CSVec2Sub(const v1, v2: TCSVec2): TCSVec2;
begin
  Result.x := v1.x - v2.x;
  Result.y := v1.y - v2.y;
end;

function CSVec2Scale(const v: TCSVec2; const s: Single): TCSVec2;
begin
  Result.x := v.x * s;
  Result.y := v.y * s;
end;

function CSVec2Dot(const v1, v2: TCSVec2): Single;
begin
  Result := v1.x * v2.x + v1.y * v2.y;
end;

function CSVec2Cross(const v1, v2: TCSVec2): Single;
begin
  Result := v1.x * v2.y - v1.y * v2.x;
end;

function CSVec2Dist(const v1, v2: TCSVec2): Single;
asm
  movlps xmm0, [eax]
  movlps xmm1, [edx]
  subps xmm0, xmm1
  mulps xmm0, xmm0
  unpcklps xmm0, xmm0
  movhlps xmm1, xmm0
  addss xmm0, xmm1
  sqrtss xmm0, xmm0
  movss [Result], xmm0
end;

function CSVec2DistSq(const v1, v2: TCSVec2): Single;
begin
  Result := Sqr(v1.x - v2.x) + Sqr(v1.y - v2.y);
end;

function CSVec2Angle(const v1, v2: TCSVec2): Single;
asm
  movlps xmm0, [eax]
  movlps xmm1, [edx]
  movaps xmm2, xmm0
  movlhps xmm2, xmm1
  mulps xmm2, xmm2
  movaps xmm3, xmm2
  shufps xmm3, xmm3, $B1
  addps xmm2, xmm3
  sqrtps xmm2, xmm2
  movhlps xmm3, xmm2
  mulss xmm2, xmm3
  mulps xmm0, xmm1
  movaps xmm1, xmm0
  unpcklps xmm1, xmm1
  movhlps xmm1, xmm1
  addss xmm0, xmm1
  rcpss xmm2, xmm2
  mulss xmm0, xmm2
  movss [Result], xmm0
  fld1
  fld [Result]
  fst st(2)
  fmul st(0), st(0)
  fsubp
  fsqrt
  fxch
  fpatan
  fstp [Result]
end;

function CSVec2Perp(const v: TCSVec2): TCSVec2;
begin
  Result.x := -v.y;
  Result.y := v.x;
end;

function CSVec2Reflect(const v, n: TCSVec2): TCSVec2;
var
  Dot: Single;
begin
  Dot := CSVec2Dot(v, n);
  Result := CSVec2(v.x - 2 * n.x * Dot, v.y - 2 * n.y * Dot);
end;

function CSVec2Lerp(const v1, v2: TCSVec2; const s: Single): TCSVec2;
begin
  Result.x := v1.x + s * (v2.x - v1.x);
  Result.y := v1.y + s * (v2.y - v1.y);
end;

function CSVec2Mat2Mul(const v: TCSVec2; const m: TCSMat2): TCSVec2;
var
  vr: TCSVec2;
begin
  vr.x := v.x * m.e00 + v.y * m.e10;
  vr.y := v.x * m.e01 + v.y * m.e11;
  Result := vr;
end;

function CSVec2Mat3Mul(const v: TCSVec2; const m: TCSMat3): TCSVec2;
var
  vr: TCSVec2;
begin
  vr.x := v.x * m.e00 + v.y * m.e10 + m.e20;
  vr.y := v.x * m.e01 + v.y * m.e11 + m.e21;
  Result := vr;
end;

function CSVec2ToStr(const v: TCSVec2): AnsiString;
begin
  Result := 'CSVec2(' + FormatFloat('0.00', v.x) + ', ' + FormatFloat('0.00', v.y) + ')';
end;

function CSMat2(const e00, e01, e10, e11: Single): TCSMat2;
begin
  Result.e00 := e00;
  Result.e01 := e01;
  Result.e10 := e10;
  Result.e11 := e11;
end;

function CSMat2Identity: TCSMat2;
begin
  Result.e00 := 1;
  Result.e01 := 0;
  Result.e10 := 0;
  Result.e11 := 1;
end;

function CSMat2Scaling(const sx, sy: Single): TCSMat2;
begin
  Result.e00 := sx;
  Result.e10 := 0;
  Result.e01 := 0;
  Result.e11 := sy;
end;

function CSMat2Rotation(const r: Single): TCSMat2;
var
  s, c: single;
begin
  CSSinCos(r, s, c);
  Result.e00 := c;
  Result.e10 := -s;
  Result.e01 := s;
  Result.e11 := c;
end;

function CSMat2Det(const m: TCSMat2): Single;
begin
  Result := m.e00 * m.e11 - m.e10 * m.e01;
end;

function CSMat2Add(const m1, m2: TCSMat2): TCSMat2;
begin
  Result.e00 := m1.e00 + m2.e00;
  Result.e01 := m1.e01 + m2.e01;
  Result.e10 := m1.e10 + m2.e10;
  Result.e11 := m1.e11 + m2.e11;
end;

function CSMat2Sub(const m1, m2: TCSMat2): TCSMat2;
begin
  Result.e00 := m1.e00 - m2.e00;
  Result.e01 := m1.e01 - m2.e01;
  Result.e10 := m1.e10 - m2.e10;
  Result.e11 := m1.e11 - m2.e11;
end;

function CSMat2Scale(const m: TCSMat2; s: Single): TCSMat2;
begin
  Result.e00 := m.e00 * s;
  Result.e01 := m.e01 * s;
  Result.e10 := m.e10 * s;
  Result.e11 := m.e11 * s;
end;

function CSMat2Mul(const m1, m2: TCSMat2): TCSMat2;
asm
  movups xmm0, [eax]
  movups xmm1, [edx]
  movaps xmm2, xmm0
  shufps xmm0, xmm0, $a0
  shufps xmm2, xmm2, $f5
  movaps xmm3, xmm1
  shufps xmm1, xmm1, $44
  shufps xmm3, xmm3, $ee
  mulps xmm0, xmm1
  mulps xmm2, xmm3
  addps xmm0, xmm2
  movlps qword ptr [Result], xmm0
  movhps qword ptr [Result + 8h], xmm0
end;

function CSMat2Transpose(const m: TCSMat2): TCSMat2;
var
  rm: TCSMat2;
begin
  rm.e00 := m.e00;
  rm.e01 := m.e10;
  rm.e10 := m.e01;
  rm.e11 := m.e11;
end;

function CSMat2Inverse(const m: TCSMat2): TCSMat2;
var
  d: Single;
  rm: TCSMat2;
begin
  d := 1 / CSMat2Det(m);
  rm.e00 := m.e11 * d;
  rm.e01 := -m.e01 * d;
  rm.e10 := -m.e10 * d;
  rm.e11 := m.e00 * d;
  Result := rm;
end;

function CSMat2ExtRotation(const m: TCSMat2): Single;
begin
  Result := CSArcTan2(m.e01, m.e00);
end;

function CSMat2ToStr(const m: TCSMat2): AnsiString;
begin
  Result := 'CSMat2(' +
    'e00:' + FormatFloat('0.00', m.e00) +
    ', e01:' + FormatFloat('0.00', m.e01) +
    ', e10:' + FormatFloat('0.00', m.e10) +
    ', e11:' + FormatFloat('0.00', m.e11) +
  ')';
end;

function CSMat3(const e00, e01, e10, e11, e20, e21: Single): TCSMat3;
begin
  Result.e00 := e00;
  Result.e01 := e01;
  Result.e10 := e10;
  Result.e11 := e11;
  Result.e20 := e20;
  Result.e21 := e21;
end;

function CSMat3Identity: TCSMat3;
begin
  Result.e00 := 1;
  Result.e01 := 0;
  Result.e10 := 0;
  Result.e11 := 1;
  Result.e20 := 0;
  Result.e21 := 0;
end;

function CSMat3Translation(const x, y: Single): TCSMat3;
begin
  Result.e00 := 1;
  Result.e01 := 0;
  Result.e10 := 0;
  Result.e11 := 1;
  Result.e20 := x;
  Result.e21 := y;
end;

function CSMat3Scaling(const sx, sy: Single): TCSMat3;
begin
  Result.e00 := sx;
  Result.e01 := 0;
  Result.e10 := 0;
  Result.e11 := sy;
  Result.e20 := 0;
  Result.e21 := 0;
end;

function CSMat3Rotation(const r: Single): TCSMat3;
var
  s, c: single;
begin
  CSSinCos(r, s, c);
  Result.e00 := c;
  Result.e01 := s;
  Result.e10 := -s;
  Result.e11 := c;
  Result.e20 := 0;
  Result.e21 := 0;
end;

function CSMat3Det(const m: TCSMat3): Single;
begin
  Result := m.e00 * m.e11 - m.e10 * m.e01;
end;

function CSMat3Add(const m1, m2: TCSMat3): TCSMat3;
begin
  Result.e00 := m1.e00 + m2.e00;
  Result.e01 := m1.e01 + m2.e01;
  Result.e10 := m1.e10 + m2.e10;
  Result.e11 := m1.e11 + m2.e11;
  Result.e20 := m1.e20 + m2.e20;
  Result.e21 := m1.e21 + m2.e21;
end;

function CSMat3Sub(const m1, m2: TCSMat3): TCSMat3;
begin
  Result.e00 := m1.e00 - m2.e00;
  Result.e01 := m1.e01 - m2.e01;
  Result.e10 := m1.e10 - m2.e10;
  Result.e11 := m1.e11 - m2.e11;
  Result.e20 := m1.e20 - m2.e20;
  Result.e21 := m1.e21 - m2.e21;
end;

function CSMat3Scale(const m: TCSMat3; s: Single): TCSMat3;
begin
  Result.e00 := m.e00 * s;
  Result.e01 := m.e01 * s;
  Result.e10 := m.e10 * s;
  Result.e11 := m.e11 * s;
  Result.e20 := m.e20 * s;
  Result.e21 := m.e21 * s;
end;

function CSMat3Mul(const m1, m2: TCSMat3): TCSMat3;
asm
  movups xmm0, [eax]
  movaps xmm2, xmm0
  shufps xmm0, xmm0, $a0
  shufps xmm2, xmm2, $f5
  movups xmm1, [edx]
  movaps xmm3, xmm1
  shufps xmm1, xmm1, $44
  shufps xmm3, xmm3, $ee
  mulps xmm0, xmm1
  mulps xmm2, xmm3
  addps xmm0, xmm2
  movlps qword ptr [Result], xmm0
  movhps qword ptr [Result + 8h], xmm0
  movlps xmm0, [edx + 10h]
  movlps xmm2, [eax + 10h]
  unpcklps xmm2, xmm2
  movlhps xmm1, xmm3
  mulps xmm1, xmm2
  movhlps xmm2, xmm1
  addps xmm0, xmm1
  addps xmm0, xmm2
  movlps qword ptr [Result + 10h], xmm0
end;

function CSMat3Inverse(const m: TCSMat3): TCSMat3;
var
  d: Single;
  rm: TCSMat3;
begin
  d := 1 / CSMat3Det(m);
  rm.e00 := m.e11 * d;
  rm.e01 := -m.e01 * d;
  rm.e10 := -m.e10 * d;
  rm.e11 := m.e00 * d;
  rm.e20 := -m.e20;
  rm.e21 := -m.e21;
  Result := rm;
end;

function CSMat3ExtRotation(const m: TCSMat3): Single;
begin
  Result := CSArcTan2(m.e01, m.e00);
end;

function CSMat3ToStr(const m: TCSMat3): AnsiString;
begin
  Result := 'CSMat3(' +
    'e00:' + FormatFloat('0.00', m.e00) +
    ', e01:' + FormatFloat('0.00', m.e01) +
    ', e10:' + FormatFloat('0.00', m.e10) +
    ', e11:' + FormatFloat('0.00', m.e11) +
    ', e20:' + FormatFloat('0.00', m.e21) +
    ', e21:' + FormatFloat('0.00', m.e20) +
  ')';
end;

function CSPoly(const n: Integer; const r: Single): TCSPoly;
var
  i: Integer;
  s, c, rcpn: Single;
begin
  SetLength(Result.v, n);
  rcpn := 1 / n;
  for i := 0 to n - 1 do
  begin
    CSSinCos(i * rcpn * CSTwoPi, s, c);
    Result.v[i].x := c * r;
    Result.v[i].y := s * r;
  end;
end;

procedure CSPolyEdge(const p: TCSPoly; const EdgeIndex: Integer; var Index1, Index2: Integer);
begin
  Index1 := EdgeIndex mod Length(p.v);
  Index2 := (EdgeIndex + 1) mod Length(p.v);
end;

function CSPolyEdgeDir(const p: TCSPoly; const EdgeIndex: Integer): TCSVec2;
var
  Ind1, Ind2: integer;
begin
  CSPolyEdge(p, EdgeIndex, Ind1, Ind2);
  Result := CSVec2Norm(CSVec2Sub(p.v[Ind2], p.v[Ind1]));
end;

function CSPolyEdgeNormal(const p: TCSPoly; const EdgeIndex: Integer): TCSVec2;
begin
  Result := CSVec2Perp(CSPolyEdgeDir(p, EdgeIndex));
end;

function CSPolyCenter(const p: TCSPoly): TCSVec2;
var
  i: Integer;
  MinV, MaxV: TCSVec2;
begin
  MinV := p.v[0];
  MaxV := MinV;
  for i := 1 to High(p.v) do
  begin
    if p.v[i].x < MinV.x then MinV.x := p.v[i].x;
    if p.v[i].x > MaxV.x then MaxV.x := p.v[i].x;
    if p.v[i].y < MinV.y then MinV.y := p.v[i].y;
    if p.v[i].y > MaxV.y then MaxV.y := p.v[i].y;
  end;
  Result := CSVec2Scale(CSVec2Add(MinV, MaxV), 0.5);
end;

function CSPolyBounds(const p: TCSPoly): TCSBounds;
var
  i: Integer;
  MinV, MaxV: TCSVec2;
begin
  MinV := p.v[0];
  MaxV := MinV;
  for i := 1 to High(p.v) do
  begin
    if p.v[i].x < MinV.x then MinV.x := p.v[i].x;
    if p.v[i].x > MaxV.x then MaxV.x := p.v[i].x;
    if p.v[i].y < MinV.y then MinV.y := p.v[i].y;
    if p.v[i].y > MaxV.y then MaxV.y := p.v[i].y;
  end;
  Result.c := CSVec2Scale(CSVec2Add(MinV, MaxV), 0.5);
  Result.nx.SetValue((MaxV.x - MinV.x) * 0.5, 0);
  Result.ny.SetValue(0, (MaxV.y - MinV.y) * 0.5);
end;

procedure CSPolyMove(const p: TCSPoly; const Pos: TCSVec2);
var
  i: Integer;
begin
  for i := 0 to High(p.v) do
  p.v[i] := CSVec2Add(p.v[i], Pos);
end;

function CSPolyTransform(const p: TCSPoly; const m: TCSMat2): TCSPoly;
var
  i: Integer;
begin
  for i := 0 to High(p.v) do
  Result.v[i] := CSVec2Mat2Mul(p.v[i], m);
end;

function CSPolyTransform(const p: TCSPoly; const m: TCSMat3): TCSPoly;
var
  i: Integer;
begin
  for i := 0 to High(p.v) do
  Result.v[i] := CSVec2Mat3Mul(p.v[i], m);
end;

function CSCircle(const c: TCSVec2; const r, a: Single): TCSCircle;
begin
  Result.c := c;
  Result.r := r;
  Result.a := a;
end;

function CSResp(const n: TCSVec2; const d: Single): TCSResp;
begin
  Result.n := n;
  Result.d := d;
end;

function CSCont(const p: TCSVec2; const n: TCSVec2): TCSCont;
begin
  Result.p := p;
  Result.n := n;
end;

function CSBounds(const c, nx, ny: TCSVec2): TCSBounds;
begin
  Result.c := c;
  Result.nx := nx;
  Result.ny := ny;
end;

function CSBoundsMat3Mul(const b: TCSBounds; const m: TCSMat3): TCSBounds;
begin
  Result.c := CSVec2Mat3Mul(b.c, m);
  Result.nx := CSVec2Mat2Mul(b.nx, PCSMat2(@m)^);
  Result.ny := CSVec2Mat2Mul(b.ny, PCSMat2(@m)^);
end;

procedure CSPolyProject(const a: TCSVec2; const p: TCSPoly; var Min, Max: Single);
var
  d: Single;
  i: Integer;
begin
  d := CSVec2Dot(a, p.v[0]);
  Min := d;
  Max := d;
  for i := 1 to High(p.v) do
  begin
    d := CSVec2Dot(a, p.v[i]);
    if d < Min then Min := d;
    if d > Max then Max := d;
  end;
end;

procedure CSCircleProject(const a: TCSVec2; const c: TCSCircle; var Min, Max: Single);
begin
  Min := CSVec2Dot(a, c.c);
  Max := Min + c.r;
  Min := Min - c.r;
end;

function CSAxisSeparatePoly(const a: TCSVec2; const p1, p2: TCSPoly; const r: PCSResp = nil): Boolean;
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSPolyProject(a, p1, Min1, Max1);
  CSPolyProject(a, 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 := a;
    r^.d := Abs(d);
  end;
end;

function CSAxisSeparateCircle(const a: TCSVec2; const c1, c2: TCSCircle; const r: PCSResp = nil): Boolean;
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSCircleProject(a, c1, Min1, Max1);
  CSCircleProject(a, 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 := a;
    r^.d := Abs(d);
  end;
end;

function CSAxisSeparatePolyCircle(const a: TCSVec2; const p: TCSPoly; const c: TCSCircle; const r: PCSResp = nil): Boolean; {inline;}
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSPolyProject(a, p, Min1, Max1);
  CSCircleProject(a, 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 := a;
    r^.d := Abs(d);
  end;
end;

function CSAxisSeparateCirclePoly(const a: TCSVec2; const c: TCSCircle; const p: TCSPoly; const r: PCSResp = nil): Boolean; {inline;}
var
  Min1, Max1, Min2, Max2: single;
  d, d1, d2: single;
begin
  CSCircleProject(a, c, Min1, Max1);
  CSPolyProject(a, 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 := a;
    r^.d := Abs(d);
  end;
end;

function CSIntersectPoly(const p1, p2: TCSPoly; const r: PCSResp = nil): Boolean;
var
  i: Integer;
  n: TCSVec2;
  CurResp: Integer;
begin
  Result := True;
  i := Length(p1.v) + Length(p2.v);
  if Length(g_RespArr) < i then
  SetLength(g_RespArr, i);
  CurResp := 0;
  for i := 0 to High(p1.v) do
  begin
    n := CSPolyEdgeNormal(p1, i);
    if CSAxisSeparatePoly(n, p1, p2, @g_RespArr[CurResp]) then
    begin
      Result := False;
      Exit;
    end;
    Inc(CurResp);
  end;
  for i := 0 to High(p2.v) do
  begin
    n := CSPolyEdgeNormal(p2, i);
    if CSAxisSeparatePoly(n, p1, p2, @g_RespArr[CurResp]) then
    begin
      Result := False;
      Exit;
    end;
    Inc(CurResp);
  end;
  if Assigned(r) then
  begin
    r^ := CSFindMTD(CurResp);
    if CSVec2Dot(CSVec2Sub(CSPolyCenter(p1), CSPolyCenter(p2)), r^.n) < 0 then
    r^.n := -r^.n;
  end;
end;

function CSIntersectCircle(const c1, c2: TCSCircle; const r: PCSResp = nil): Boolean;
var
  Axis: TCSVec2;
begin
  Axis := CSVec2Sub(c1.c, c2.c);
  Axis := CSVec2Norm(Axis);
  Result := not CSAxisSeparateCircle(Axis, c1, c2, r);
end;

function CSIntersectPolyCircle(const p: TCSPoly; const c: TCSCircle; const r: PCSResp = nil): Boolean;
var
  i: Integer;
  n: TCSVec2;
  CurResp: Integer;
  d, md: Single;
  MinVecInd: Integer;
begin
  Result := True;
  i := Length(p.v) + 1;
  if Length(g_RespArr) < i then
  SetLength(g_RespArr, i);
  CurResp := 0;
  for i := 0 to High(p.v) do
  begin
    n := CSPolyEdgeNormal(p, i);
    if CSAxisSeparatePolyCircle(n, p, c, @g_RespArr[CurResp]) then
    begin
      Result := False;
      Exit;
    end;
    Inc(CurResp);
  end;
  MinVecInd := 0;
  d := CSVec2DistSq(c.c, p.v[0]);
  md := d;
  for i := 1 to High(p.v) do
  begin
    d := CSVec2DistSq(c.c, p.v[i]);
    if d < md then
    begin
      md := d;
      MinVecInd := i;
    end;
  end;
  if CSAxisSeparatePolyCircle(CSVec2Norm(CSVec2Sub(c.c, p.v[MinVecInd])), p, c, @g_RespArr[CurResp]) then
  begin
    Result := False;
    Exit;
  end;
  Inc(CurResp);
  if Assigned(r) then
  begin
    r^ := CSFindMTD(CurResp);
    if CSVec2Dot(CSVec2Sub(CSPolyCenter(p), c.c), r^.n) < 0 then
    r^.n := -r^.n;
  end;
end;

function CSIntersectCirclePoly(const c: TCSCircle; const p: TCSPoly; const r: PCSResp = nil): Boolean;
var
  i: Integer;
  n: TCSVec2;
  CurResp: Integer;
  d, md: Single;
  MinVecInd: Integer;
begin
  Result := True;
  i := Length(p.v) + 1;
  if Length(g_RespArr) < i then
  SetLength(g_RespArr, i);
  CurResp := 0;
  for i := 0 to High(p.v) do
  begin
    n := CSPolyEdgeNormal(p, i);
    if CSAxisSeparateCirclePoly(n, c, p, @g_RespArr[CurResp]) then
    begin
      Result := False;
      Exit;
    end;
    Inc(CurResp);
  end;
  MinVecInd := 0;
  d := CSVec2DistSq(c.c, p.v[0]);
  md := d;
  for i := 1 to High(p.v) do
  begin
    d := CSVec2DistSq(c.c, p.v[i]);
    if d < md then
    begin
      md := d;
      MinVecInd := i;
    end;
  end;
  if CSAxisSeparateCirclePoly(CSVec2Norm(CSVec2Sub(p.v[MinVecInd], c.c)), c, p, @g_RespArr[CurResp]) then
  begin
    Result := False;
    Exit;
  end;
  Inc(CurResp);
  if Assigned(r) then
  begin
    r^ := CSFindMTD(CurResp);
    if CSVec2Dot(CSVec2Sub(c.c, CSPolyCenter(p)), r^.n) < 0 then
    r^.n := -r^.n;
  end;
end;

procedure CSFindSupportPointsPoly(const n: TCSVec2; const p: TCSPoly; const List: PCSList);
var
  i: Integer;
  d, pd: Single;
const
  Threshold = 1;
begin
  d := CSVec2Dot(p.v[0], n);
  List.Add(@p.v[0]);
  for i := 1 to High(p.v) do
  begin
    pd := CSVec2Dot(p.v[i], n);
    if pd >= d - Threshold then
    begin
      if pd > d + Threshold then
      begin
        List^.Count := 0;
        d := pd;
      end;
      List.Add(@p.v[i]);
    end;
  end;
end;

procedure CSFindContactsPoly(const n: TCSVec2; const p1, p2: TCSPoly; const Cont: PCSContGroup);
var
  i: Integer;
  d, Min1, Max1, Min2, Max2: Single;
  Perp: TCSVec2;
  Mag: Single;
begin
  g_sp1.Count := 0;
  g_sp2.Count := 0;
  g_sp3.Count := 0;
  CSFindSupportPointsPoly(-n, p1, @g_sp1);
  CSFindSupportPointsPoly(n, p2, @g_sp2);
  if (g_sp1.Count = 1) and (g_sp2.Count = 1) then
  begin
    Cont^.Count := 1;
    Cont^.c[0].n := n;
    Cont^.c[0].p := CSVec2Scale(CSVec2Add(PCSVec2(g_sp1[0])^, PCSVec2(g_sp2[0])^), 0.5);
  end
  else
  if (g_sp1.Count = 1) and (g_sp2.Count > 1) then
  begin
    Cont^.Count := 1;
    Cont^.c[0].n := n;
    Cont^.c[0].p := PCSVec2(g_sp1[0])^;
  end
  else
  if (g_sp1.Count > 1) and (g_sp2.Count = 1) then
  begin
    Cont^.Count := 1;
    Cont^.c[0].n := n;
    Cont^.c[0].p := PCSVec2(g_sp2[0])^;
  end
  else
  begin
    Perp := CSVec2Perp(n);
    Min1 := CSVec2Dot(PCSVec2(g_sp1[0])^, Perp);
    Max1 := Min1;
    Min2 := CSVec2Dot(PCSVec2(g_sp2[0])^, Perp);
    Max2 := Min2;
    for i := 1 to g_sp1.Count - 1 do
    begin
      d := CSVec2Dot(PCSVec2(g_sp1[i])^, Perp);
      if d < Min1 then Min1 := d;
      if d > Max1 then Max1 := d;
    end;
    for i := 1 to g_sp2.Count - 1 do
    begin
      d := CSVec2Dot(PCSVec2(g_sp2[i])^, Perp);
      if d < Min2 then Min2 := d;
      if d > Max2 then Max2 := d;
    end;
    for i := 0 to g_sp1.Count - 1 do
    begin
      d := CSVec2Dot(PCSVec2(g_sp1[i])^, Perp);
      if (d >= Min2) and (d <= Max2) then
      g_sp3.Add(g_sp1[i]);
    end;
    for i := 0 to g_sp2.Count - 1 do
    begin
      d := CSVec2Dot(PCSVec2(g_sp2[i])^, Perp);
      if (d >= Min1) and (d <= Max1) then
      g_sp3.Add(g_sp2[i]);
    end;
    if Cont^.Capacity < g_sp3.Count then
    Cont^.Capacity := g_sp3.Count;
    Cont^.Count := g_sp3.Count;
    for i := 0 to Cont^.Count - 1 do
    begin
      Cont^.c[i].n := n;
      Cont^.c[i].p := PCSVec2(g_sp3[i])^;
    end;
  end;
end;

procedure CSFindContactsCircle(const n: TCSVec2; const c1, c2: TCSCircle; const Cont: PCSContGroup);
begin
  Cont^.Count := 1;
  Cont^.c[0].p := c2.c + n * c2.r;
  Cont^.c[0].n := n;
end;

procedure CSFindContactsPolyCircle(const n: TCSVec2; const p: TCSPoly; const c: TCSCircle; const Cont: PCSContGroup);
begin
  Cont^.Count := 1;
  Cont^.c[0].p := c.c + n * c.r;
  Cont^.c[0].n := n;
end;

procedure CSFindContactsCirclePoly(const n: TCSVec2; const c: TCSCircle; const p: TCSPoly; const Cont: PCSContGroup);
begin
  Cont^.Count := 1;
  Cont^.c[0].p := c.c - n * c.r;
  Cont^.c[0].n := n;
end;

function CSFindMTD(const RCount: Integer): TCSResp;
var
  ResultInd, i: Integer;
  md: Single;
begin
  ResultInd := 0;
  md := g_RespArr[0].d;
  for i := 1 to RCount - 1 do
  begin
    if g_RespArr[i].d < md then
    begin
      md := g_RespArr[i].d;
      ResultInd := i;
    end;
  end;
  Result := g_RespArr[ResultInd];
end;

function CSGetForceAtPoint(const Body: TCSBody; const p: TCSVec2): TCSVec2;
begin
  if Body.IsStatic then
  begin
    Result.SetValue(0, 0);
    Exit;
  end;
  Result := CSVec2Add(CSVec2Scale(CSVec2Perp(CSVec2Sub(p, PCSVec2(@Body.Transform.e20)^)), Body.VelAng), Body.VelLin) * Body.Mass;
end;

procedure CSSetForceAtPoint(const Body: TCSBody; const p, f: TCSVec2);
var
  ModelPoint: TCSVec2;
  m: TCSVec2;
  d: Single;
begin
  if Body.IsStatic then Exit;
  ModelPoint := CSVec2Sub(p, PCSVec2(@Body.Transform.e20)^);
  m := f / Body.Mass;
  Body.VelLin := CSVec2Add(Body.VelLin, CSVec2Scale(m, Abs(CSVec2Dot(CSVec2Norm(ModelPoint), CSVec2Norm(m)))));
  d := CSVec2Len(ModelPoint);
  if d <= 1 then Exit;
  Body.VelAng := Body.VelAng + ((CSVec2Dot(CSVec2Norm(CSVec2Perp(ModelPoint)), m)) / d);
end;

procedure CSResolveIntersection(const Body1, Body2: TCSBody; const r: TCSResp);
var
  tm, m1, m2: Single;
begin
  if Body1.IsStatic and Body2.IsStatic then
  Exit
  else if Body1.IsStatic then
  PCSVec2(@Body2.Transform.e20)^ := CSVec2Add(PCSVec2(@Body2.Transform.e20)^, CSVec2Scale(r.n, -r.d))
  else if Body2.IsStatic then
  PCSVec2(@Body1.Transform.e20)^ := CSVec2Add(PCSVec2(@Body1.Transform.e20)^, CSVec2Scale(r.n, r.d))
  else
  begin
    tm := 1 / (Body1.Mass + Body2.Mass);
    m1 := Body1.Mass * tm;
    m2 := Body2.Mass * tm;
    PCSVec2(@Body1.Transform.e20)^ := CSVec2Add(PCSVec2(@Body1.Transform.e20)^, CSVec2Scale(r.n, r.d * m2));
    PCSVec2(@Body2.Transform.e20)^ := CSVec2Add(PCSVec2(@Body2.Transform.e20)^, CSVec2Scale(r.n, -r.d * m1));
  end;
end;

function CSResolveForces(
    const N1, N2, F1, F2: TCSVec2;
    var Direct1: TCSVec2;
    var Friction1: TCSVec2;
    var Direct2: TCSVec2;
    var Friction2: TCSVec2;
    const MagHardness: Single = 1;
    const MagFriction: Single = 1
  ): Boolean;
var
  FD1, FD2, FF1, FF2, FDR: Single;
  NF1, NF2: TCSVec2;
begin
  FD1 := CSVec2Dot(N1, F1);
  FD2 := CSVec2Dot(N2, F2);
  FDR := FD1 + FD2;
  if FDR <= 0 then
  begin
    Direct1.SetValue(0, 0);
    Friction1.SetValue(0, 0);
    Direct2.SetValue(0, 0);
    Friction2.SetValue(0, 0);
    Result := False;
    Exit;
  end;
  NF1 := CSVec2Perp(N1);
  NF2 := -NF1;
  FF1 := CSVec2Dot(F1, NF1);
  FF2 := CSVec2Dot(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;

procedure CSResolveContacts(const Body1, Body2: TCSBody; const Contacts: TCSContGroup; const r: TCSResp);
var
  TM, M1, M2, TH, TF, CM: Single;
  FTotal1, FDirect1, FFriction1: TCSVec2;
  FTotal2, FDirect2, FFriction2: TCSVec2;
  F: TCSVec2;
  i: Integer;
begin
  if (Contacts.Count = 0)
  or (Body1.IsStatic and Body2.IsStatic) then Exit;
  TH := Body1.Hardness * Body2.Hardness;
  TF := Body1.Friction * Body2.Friction;

  CM := 1 / Contacts.Count;
  for i := 0 to Contacts.Count - 1 do
  begin
    FTotal1 := CSGetForceAtPoint(Body1, Contacts.c[i].p);
    FTotal2 := CSGetForceAtPoint(Body2, Contacts.c[i].p);
    if CSResolveForces(
      -Contacts.c[i].n, Contacts.c[i].n,
      FTotal1, FTotal2,
      FDirect1, FFriction1,
      FDirect2, FFriction2,
      TH, TF
    ) then
    begin
      F := (FDirect2 + FFriction2 - FDirect1 - FFriction1) * CM;
      CSSetForceAtPoint(Body1, Contacts.c[i].p, F);
      CSSetForceAtPoint(Body2, Contacts.c[i].p, -F);
    end;
  end;
end;

initialization
begin
  g_sp1.Capacity := 16;
  g_sp2.Capacity := 16;
  g_sp3.Capacity := 16;
end;

end.