unit AnimationrigPhysicsSpring;

interface

uses SysUtils, Classes, Contnrs, math,
  AnimationrigMisc, AnimationrigWiggle;

type
  TPhysicsMatrix4 = TAnimrigMisc.Matrix4;

type
  TPhysicsVector3 = record
    x, y, z: double;

    function Length: double; inline;
    procedure Normalize; inline;
    procedure SetVector(x_, y_, z_: double); inline;
    procedure SetSub(const v1, v2: TPhysicsVector3); inline;
    procedure SetAdd(const v1, v2: TPhysicsVector3); inline;
    procedure SetBlend(const v1, v2: TPhysicsVector3; blend:single); inline;
    procedure Mul(m: double); inline;
    procedure Transform(const m: TPhysicsMatrix4); inline;
    function DotProduct(const v1: TPhysicsVector3): double; inline;
    function Compare(const v1: TPhysicsVector3): boolean; inline;
  end;

  // base
  TPhysicsNodeType = (ntPoint, ntBone, ntElement);
  TPhysicsSpringModel = (smOneSide, smBoth);
  TPhysicsGroupType = (gtSpring,gtElement);

  TPhysicsGroup = class
  protected
    fGroupType: TPhysicsGroupType;
  public
    Name: string;
    AliasName: string;

    constructor Create;
    property GroupType: TPhysicsGroupType read fGroupType;
    procedure Update(dt: double); virtual; abstract;
    procedure Apply; virtual; abstract;
    procedure Appease; virtual; abstract;
    procedure AddForce(const d: TPhysicsVector3); virtual; abstract;
  end;

  TPhysicsNode = class
  protected
    fNodeType: TPhysicsNodeType;
  public
    UserObject: pointer;
    WorldPosition: TPhysicsVector3;
    Fixed: boolean;

    // Mass : double;
    constructor Create;
    property NodeType: TPhysicsNodeType read fNodeType;
  end;

  TPhysicsSpring = class
  protected
    fNodeType: TPhysicsNodeType;
    fOwnerGroup: TPhysicsGroup;
  public
    constructor Create(group: TPhysicsGroup);
    procedure Update(dt: double); virtual; abstract;
    procedure SetNatural(dv: TPhysicsVector3); virtual; abstract;
    procedure Appease; virtual; abstract;

    property NodeType: TPhysicsNodeType read fNodeType;
  end;

  // basic spring param
  TPhysicsSpringGroup = class(TPhysicsGroup)
  public
    K: double;
    Damping: double;
    Gravity: double;
    Mass: double;
    GravityVector: TPhysicsVector3;
    LimitVelocity: double;
    LimitAngleMin, LimitAngleMax: double;
    LimitMoveRatio: double;

    constructor Create;
  end;

  TPhysicsSpringVector = class(TPhysicsSpring)
  protected
    fNode1, fNode2: TPhysicsNode;
    fNatural: TPhysicsVector3;
    fVelocity: TPhysicsVector3;
    fNaturalAngle, fAppliedAngle: double;
    fParent: TPhysicsSpringVector;
  public
    FixedLength: boolean;
    SpringMode: TPhysicsSpringModel;

    constructor Create(group: TPhysicsGroup; parent_: TPhysicsSpringVector);
    procedure Update(dt: double); override;
    procedure SetNatural(v: TPhysicsVector3); override;
    procedure Appease; override;

    property Natural: TPhysicsVector3 read fNatural;
    property NaturalAngle: double read fNaturalAngle;
    property AppliedAngle: double read fAppliedAngle;
    property Velocity: TPhysicsVector3 read fVelocity;
    property Parent: TPhysicsSpringVector read fParent;
  end;

  // basic element spring param
  TPhysicsElementGroup = class(TPhysicsGroup)
  public
    K: double;
    Damping: double;
    Mass: double;
    LimitVelocity: double;
    // LimitAngleMin,LimitAngleMax : double;
    // LimitMoveRatio: double;

    constructor Create;
  end;
  TPhysicsSpringElement = class(TPhysicsSpring)
  protected
    fVelocity: TPhysicsVector3;
    fStartupFlag : boolean;
  public
    Mask : integer;
    Alpha : single;
    Value: TPhysicsVector3;
    aValue: TPhysicsVector3;
    pValue : TPhysicsVector3;

    constructor Create(group: TPhysicsGroup);
    procedure Update(dt: double); override;
    procedure SetNatural(v: TPhysicsVector3); override;
    procedure Appease; override;
  end;

  // point (softbody)
  TPhysicsNodePoint = class(TPhysicsNode)
  public
    constructor Create;
  end;

  TPhysicsSpringPoint = class(TPhysicsSpringVector)
  protected
    fPoint1, fPoint2: TPhysicsNodePoint;
  public
    constructor Create(g: TPhysicsGroup; parent_: TPhysicsSpringPoint;
      p1, p2: TPhysicsNodePoint);
    procedure SetupNatural;

    property Point1: TPhysicsNodePoint read fPoint1;
    property Point2: TPhysicsNodePoint read fPoint2;
  end;

  // bone
  TPhysicsNodeBone = class(TPhysicsNode)
  protected
    fParent: TPhysicsNodeBone;
    fChildren: TList;
    fSprings: TList;
  public
    // original
    LocalLength: double;
    LocalPosition: TPhysicsVector3;
    LocalAngle: double;
    LocalSizeX, LocalSizeY: double;
    // applied spring physics
    aLocalPosition: TPhysicsVector3;
    aLocalAngle: double;

    WorldMatrix: TPhysicsMatrix4;

    constructor Create(parentbone: TPhysicsNodeBone);
    destructor Destroy; override;
    procedure UpdateWorldMatrix(const parentmatrix: TPhysicsMatrix4;
      setupworldposition: boolean); // need call parent
    function GetRoot: TPhysicsNodeBone;
    function GetLocalMatrix: TPhysicsMatrix4;
    // function GetInvertLocalMatrix : TPysicsMatrix4;
    function GetAppliedLocalMatrix: TPhysicsMatrix4;

    property Parent: TPhysicsNodeBone read fParent;
    property Children: TList read fChildren;
    property Springs: TList read fSprings;
  end;

  // spring model
  TPhysicsSpringBone = class(TPhysicsSpringVector)
  protected
    fOwner, fChild: TPhysicsNodeBone;
    fOffsetAngle: double; // Difference between bone.angle and childbone.positon(spring line)
  public
    LockBoneAngle: boolean;
    LockBonePosition: boolean;

    constructor Create(g: TPhysicsGroup; parent_: TPhysicsSpringBone;
      ownerbone, childbone: TPhysicsNodeBone);
    procedure Update(dt: double); override; // need call parent
    procedure SetupNatural(const parentmatrix: TPhysicsMatrix4);
    // need call parent
    procedure UpdateBoneLocal(const invparentmatrix: TPhysicsMatrix4);
    // need call parent

    property Owner: TPhysicsNodeBone read fOwner;
    property Child: TPhysicsNodeBone read fChild;
  end;

  TPhysicsGroupSpringBoneMode = (gsmBone, gsmStrBone, gsmPoint, gsmStrPoint);

  TPhysicsGroupSpringBone = class(TPhysicsSpringGroup)
  protected
    fNodes: TObjectList;
    fSprings: TObjectList;
    fRootBone: TPhysicsNodeBone;
    fMode: TPhysicsGroupSpringBoneMode;
    // force
    fCacheInitFlag: boolean;
    fCacheRootPosition: TPhysicsVector3;
    fCacheRootAngle: double;

    procedure UpdateTransformForce;
    function InvMatrix2D(const m: TPhysicsMatrix4): TPhysicsMatrix4;
    procedure SlidSpring(x, y: double; bone: TPhysicsNodeBone);
  public
    ElapsedTime: double;
    ExternalMatrix: TPhysicsMatrix4;
    RootMatrix: TPhysicsMatrix4;

    ForceRotateRatio: double;
    ForceRatio: double;
    Alpha: single;
    ForceWiggle: single;

    RootUserObject: pointer;

    constructor Create;
    destructor Destroy; override;

    procedure Update(dt: double); override;
    procedure Apply; override;
    procedure Appease; override;
    procedure AddForce(const d: TPhysicsVector3); override;

    procedure AddBone(b: TPhysicsNodeBone);
    procedure AddSpring(s: TPhysicsSpringBone);

    procedure UpdateWorldMatrix;
    procedure SetupNatural;

    function GetBone(n: integer): TPhysicsNodeBone;
    function GetSpring(n: integer): TPhysicsSpringBone;

    property NodeList: TObjectList read fNodes;
    property SpringList: TObjectList read fSprings;

    property RootBone: TPhysicsNodeBone read fRootBone;
    property Mode: TPhysicsGroupSpringBoneMode read fMode write fMode;
  end;

  // element spring model
  TPhysicsGroupElementBone = class(TPhysicsElementGroup)
  protected
    fPosition : TPhysicsSpringElement;
    fScale : TPhysicsSpringElement;
    fRotate : TPhysicsSpringElement;
    fStartupFlag : boolean;
  public
    UserObject : pointer;

    constructor Create;
    destructor Destroy; override;

    procedure Update(dt: double); override;
    procedure Apply; override;
    procedure Appease; override;
    procedure AddForce(const d: TPhysicsVector3); override;

    property SpringPosition: TPhysicsSpringElement read fPosition;
    property SpringScale: TPhysicsSpringElement read fScale;
    property SpringRotate: TPhysicsSpringElement read fRotate;
  end;

implementation

{ TPhysicsVector3 }

procedure TPhysicsVector3.SetAdd(const v1, v2: TPhysicsVector3);
begin
  x := v1.x + v2.x;
  y := v1.y + v2.y;
  z := v1.z + v2.z;
end;

procedure TPhysicsVector3.SetBlend(const v1, v2: TPhysicsVector3; blend: single);
begin
  x := v1.x + (v2.x - v1.x) * blend;
  y := v1.y + (v2.y - v1.y) * blend;
  z := v1.z + (v2.z - v1.z) * blend;
end;

function TPhysicsVector3.Compare(const v1: TPhysicsVector3): boolean;
begin
  result := false;
  if (x = v1.x) and (y = v1.y) and (z = v1.z) then
    result := true;
end;

function TPhysicsVector3.DotProduct(const v1: TPhysicsVector3): double;
begin
  result := v1.x * x + v1.y * y + v1.z * z;
end;

function TPhysicsVector3.Length: double;
begin
  result := Sqrt(x * x + y * y + z * z);
end;

procedure TPhysicsVector3.Mul(m: double);
begin
  x := x * m;
  y := y * m;
  z := z * m;
end;

procedure TPhysicsVector3.Normalize;
var
  n: double;
begin
  n := Sqrt(x * x + y * y + z * z);
  if n < 0.00001 then
    exit;

  n := 1.0 / n;
  x := x * n;
  y := y * n;
  z := z * n;
end;

procedure TPhysicsVector3.SetVector(x_, y_, z_: double);
begin
  x := x_;
  y := y_;
  z := z_;
end;

procedure TPhysicsVector3.SetSub(const v1, v2: TPhysicsVector3);
begin
  x := v1.x - v2.x;
  y := v1.y - v2.y;
  z := v1.z - v2.z;
end;

procedure TPhysicsVector3.Transform(const m: TPhysicsMatrix4);
var
  xx, yy, zz: single;
begin
  xx := x;
  yy := y;
  zz := z;
  x := xx * m._11 + yy * m._21 + zz * m._31 + m._41;
  y := xx * m._12 + yy * m._22 + zz * m._32 + m._42;
  z := xx * m._13 + yy * m._23 + zz * m._33 + m._43;
end;

{ TPhysicsSpringVector }

procedure TPhysicsSpringVector.Appease;
begin
  Velocity.SetVector(0, 0, 0);
  if fNode1 = nil then
    exit;
  if fNode2 = nil then
    exit;

  fNode2.WorldPosition.x := fNode1.WorldPosition.x + Natural.x;
  fNode2.WorldPosition.y := fNode1.WorldPosition.y + Natural.y;
  fNode2.WorldPosition.z := fNode1.WorldPosition.z + Natural.z;
end;

constructor TPhysicsSpringVector.Create(group: TPhysicsGroup;
  parent_: TPhysicsSpringVector);
begin
  inherited Create(group);

  fNodeType := ntPoint;
  fParent := parent_;

  fNatural.SetVector(1.0, 0.0, 0.0);
  fVelocity.SetVector(0.0, 0.0, 0.0);
  fNaturalAngle := 0.0;
  fAppliedAngle := fNaturalAngle;

  FixedLength := false;
  SpringMode := smOneSide;

  fNode1 := nil;
  fNode2 := nil;
end;

procedure TPhysicsSpringVector.SetNatural(v: TPhysicsVector3);
begin
  fNatural := v;
  fNaturalAngle := ArcTan2(fNatural.y, fNatural.x) * TAnimrigMisc.RadianToAngle;
  fAppliedAngle := fNaturalAngle;
end;

procedure TPhysicsSpringVector.Update(dt: double);
var
  d: TPhysicsVector3;
  v: TPhysicsVector3;
  force: double;
  accel: double;
  len: double;
  Mass, imass: double;
  gx, gy, gz: double;
  K, Damping: double;
  sg: TPhysicsSpringGroup;
  p1, p2: TPhysicsVector3;
  pr, dr, pdr: double;
  m: TPhysicsMatrix4;
  targetangle: double;
begin
  sg := TPhysicsSpringGroup(fOwnerGroup);

  // spring param
  Mass := max(sg.Mass, 0.001);
  imass := 1.0 / Mass;
  K := sg.K;
  // gravity
  {
    v.SetVector(1,0,0);
    gx := v.DotProduct(sg.GravityVector) * sg.Gravity;
    v.SetVector(0,1,0);
    gy := v.DotProduct(sg.GravityVector) * sg.Gravity;
    v.SetVector(0,0,1);
    gz := v.DotProduct(sg.GravityVector) * sg.Gravity;
    }
  gx := 0.0;
  gy := 0.0;
  gz := 0.0;

  d.SetSub(fNode2.WorldPosition, fNode1.WorldPosition);

  force := -K * (d.x - fNatural.x);
  Damping := fVelocity.x * sg.Damping;
  force := force + sg.Mass * gx - Damping;
  accel := force * imass;
  fVelocity.x := fVelocity.x + accel * dt;

  force := -K * (d.y - fNatural.y);
  Damping := fVelocity.y * sg.Damping;
  force := force + sg.Mass * gy - Damping;
  accel := force * imass;
  fVelocity.y := fVelocity.y + accel * dt;

  force := -K * (d.z - fNatural.z);
  Damping := fVelocity.z * sg.Damping;
  force := force + sg.Mass * gz - Damping;
  accel := force * imass;
  fVelocity.z := fVelocity.z + accel * dt;

  if fVelocity.Length > sg.LimitVelocity * 100 then // 100 = 100fps
  begin
    fVelocity.Normalize;
    fVelocity.Mul(sg.LimitVelocity);
  end;

  // pos = pos + velocity * dt
  d := fVelocity;
  d.Mul(dt);

  if SpringMode = smBoth then
  begin
    if not(fNode1.Fixed) then
    begin
      fNode1.WorldPosition.SetSub(fNode1.WorldPosition, d);
    end;
    if not(fNode2.Fixed) then
    begin
      fNode2.WorldPosition.SetAdd(fNode2.WorldPosition, d);
    end;
  end
  else
  begin
    fNode2.WorldPosition.SetAdd(fNode2.WorldPosition, d);
  end;

  // limitter
  if (sg.LimitAngleMin > -1.0) or (sg.LimitAngleMax < +1.0) then
  begin
    // parent diff
    pdr := 0;
    if Parent <> nil then
      pdr := fParent.AppliedAngle - fParent.NaturalAngle;
    // pdr := pdr + $ff;
    // pdr := pdr - floor(pdr);
    // if pdr > 0.5 then pdr := pdr - 1.0;
    targetangle := NaturalAngle + pdr;
    // self diff
    d.SetSub(fNode2.WorldPosition, fNode1.WorldPosition);
    pr := ArcTan2(d.y, d.x) * TAnimrigMisc.RadianToAngle;
    dr := pr - targetangle;
    // bitween -0.5 to +0.5
    dr := dr + $FF;
    dr := dr - floor(dr);
    if dr > 0.5 then
      dr := dr - 1.0;

    if (dr < sg.LimitAngleMin) or (dr > sg.LimitAngleMax) then
    begin
      dr := dr;
      dr := min(max(dr, sg.LimitAngleMin), sg.LimitAngleMax);
      d.SetVector(d.Length, 0, 0);
      m := TAnimrigMisc.IdentityMatrix;
      TAnimrigMisc.m4PushRotateZ(m, targetangle + dr);
      d.Transform(m);
      fNode2.WorldPosition.SetAdd(fNode1.WorldPosition, d);
    end;
  end;
  if sg.LimitMoveRatio < $FF then
  begin
    len := fNatural.Length * sg.LimitMoveRatio; // ratio to world
    d.SetSub(fNode2.WorldPosition, fNode1.WorldPosition);
    d.SetSub(d, fNatural);
    if d.Length > len then
    begin
      d.Normalize;
      d.Mul(len);
      d.SetAdd(d, fNatural);
      fNode2.WorldPosition.SetAdd(fNode1.WorldPosition, d);
    end;
  end;

  // fixed length
  if FixedLength then
  begin
    len := fNatural.Length;
    d.SetSub(fNode2.WorldPosition, fNode1.WorldPosition);
    d.Normalize;
    d.Mul(len);
    fNode2.WorldPosition.SetAdd(fNode1.WorldPosition, d);
  end;

  // appliedangle
  d.SetSub(fNode2.WorldPosition, fNode1.WorldPosition);
  fAppliedAngle := ArcTan2(d.y, d.x) * TAnimrigMisc.RadianToAngle;
end;

{ TPhysicsNodePoint }

constructor TPhysicsNodePoint.Create;
begin
  inherited;

  fNodeType := ntPoint;
end;

{ TPhysicsSpringPoint }

constructor TPhysicsSpringPoint.Create(g: TPhysicsGroup;
  parent_: TPhysicsSpringPoint; p1, p2: TPhysicsNodePoint);
begin
  inherited Create(g, parent_);

  SpringMode := smOneSide;
  FixedLength := false;

  fNode1 := p1;
  fNode2 := p2;
  fPoint1 := p1;
  fPoint2 := p2;
end;

procedure TPhysicsSpringPoint.SetupNatural;
var
  d: TPhysicsVector3;
begin
  d.SetSub(fNode2.WorldPosition, fNode1.WorldPosition);
  SetNatural(d);
end;

{ TPhysicsNodeBone }

constructor TPhysicsNodeBone.Create(parentbone: TPhysicsNodeBone);
begin
  inherited Create;

  fNodeType := ntBone;

  fChildren := TList.Create;
  fSprings := TList.Create;

  LocalPosition.SetVector(100.0, 0.0, 0.0);
  LocalAngle := 0.0;
  LocalLength := 100.0;
  LocalSizeX := 1.0;
  LocalSizeY := 1.0;

  fParent := parentbone;
  if fParent <> nil then
    fParent.Children.Add(self);

  Fixed := fParent = nil; // root bone is fixed

  WorldMatrix := TAnimrigMisc.IdentityMatrix;
end;

destructor TPhysicsNodeBone.Destroy;
begin
  fChildren.Free;
  fSprings.Free;

  inherited;
end;

function TPhysicsNodeBone.GetAppliedLocalMatrix: TPhysicsMatrix4;
begin
  result := TAnimrigMisc.IdentityMatrix;
  TAnimrigMisc.m4PushScale(result, LocalSizeX, LocalSizeY, 1.0);
  TAnimrigMisc.m4PushRotateZ(result, aLocalAngle);
  TAnimrigMisc.m4PushMove(result, aLocalPosition.x, aLocalPosition.y,
    aLocalPosition.z);
end;

function TPhysicsNodeBone.GetLocalMatrix: TPhysicsMatrix4;
begin
  result := TAnimrigMisc.IdentityMatrix;
  TAnimrigMisc.m4PushScale(result, LocalSizeX, LocalSizeY, 1.0);
  TAnimrigMisc.m4PushRotateZ(result, LocalAngle);
  TAnimrigMisc.m4PushMove(result, LocalPosition.x, LocalPosition.y,
    LocalPosition.z);
end;

function TPhysicsNodeBone.GetRoot: TPhysicsNodeBone;
begin
  result := self;
  if Parent <> nil then
    result := Parent.GetRoot;
end;

procedure TPhysicsNodeBone.UpdateWorldMatrix
  (const parentmatrix: TPhysicsMatrix4;
  setupworldposition: boolean);
var
  v: TPhysicsVector3;
  p: pointer;
begin
  if setupworldposition then
  begin
    aLocalPosition := LocalPosition;
    aLocalAngle := LocalAngle;

    WorldMatrix := GetAppliedLocalMatrix;
    TAnimrigMisc.m43PushMatrix(WorldMatrix, parentmatrix);

    v.SetVector(0, 0, 0);
    v.Transform(WorldMatrix);
    WorldPosition := v;
  end
  else
  begin
    WorldMatrix := GetAppliedLocalMatrix;
    TAnimrigMisc.m43PushMatrix(WorldMatrix, parentmatrix);
  end;

  for p in Children do
    TPhysicsNodeBone(p).UpdateWorldMatrix(WorldMatrix, setupworldposition);
end;

{ TPhysicsNode }

constructor TPhysicsNode.Create;
begin
  fNodeType := ntPoint;

  WorldPosition.SetVector(0.0, 0.0, 0.0);
  Fixed := false;
  // Mass := _Mass;
end;

{ TPhysicsSpringBone }

constructor TPhysicsSpringBone.Create(g: TPhysicsGroup;
  parent_: TPhysicsSpringBone; ownerbone, childbone: TPhysicsNodeBone);
begin
  inherited Create(g, parent_);

  fNodeType := ntBone;
  // SpringMode := smBoth;
  SpringMode := smOneSide;

  fNatural.SetVector(100.0, 0.0, 0.0);
  fVelocity.SetVector(0.0, 0.0, 0.0);

  fOwner := ownerbone;
  fChild := childbone;
  fOwner.Springs.Add(self);

  FixedLength := true;
  LockBoneAngle := false;
  LockBonePosition := true;

  fNode1 := Owner;
  fNode2 := Child;
end;

procedure TPhysicsSpringBone.SetupNatural(const parentmatrix: TPhysicsMatrix4);
var
  m: TPhysicsMatrix4;
  v1, v2, v3, d: TPhysicsVector3;
  r: double;
  p: pointer;
begin
  m := Owner.GetLocalMatrix;
  TAnimrigMisc.m43PushMatrix(m, parentmatrix);
  v1.SetVector(0, 0, 0);
  v1.Transform(m);

  if Owner.Fixed then
  begin
    Owner.WorldPosition := v1;
  end;

  if Child = nil then
  begin
    d.SetVector(100, 0, 0);
    SetNatural(d);
  end
  else
  begin
    // next bone position
    v2 := Child.LocalPosition;
    v2.Transform(m);
    d.SetSub(v2, v1);
    SetNatural(d);
    r := ArcTan2(d.y, d.x) * TAnimrigMisc.RadianToAngle;
    // angle target position (offset)
    v3.SetVector(100, 0, 0);
    v3.Transform(m);
    d.SetSub(v3, v1);
    fOffsetAngle := ArcTan2(d.y, d.x) * TAnimrigMisc.RadianToAngle - r;
  end;

  for p in Child.Springs do
    TPhysicsSpringBone(p).SetupNatural(m);
end;

procedure TPhysicsSpringBone.Update(dt: double);
var
  p: pointer;
begin
  inherited;

  for p in Child.Springs do
    TPhysicsSpringBone(p).Update(dt);
end;

procedure TPhysicsSpringBone.UpdateBoneLocal
  (const invparentmatrix: TPhysicsMatrix4);
var
  v1, v2, d: TPhysicsVector3;
  dr: double;
  im: TPhysicsMatrix4;
  p: pointer;
begin
  if Owner = nil then
    exit;
  if Child = nil then
    exit;

  im := invparentmatrix;
  TAnimrigMisc.m4PushMove(im, -Owner.aLocalPosition.x, -Owner.aLocalPosition.y,
    -Owner.aLocalPosition.z);
  v1 := Owner.WorldPosition;
  v2 := Child.WorldPosition;
  v1.Transform(im);
  v2.Transform(im);

  d.SetSub(v2, v1);

  // angle
  if not(LockBoneAngle) then
  begin
    dr := ArcTan2(d.y, d.x) * TAnimrigMisc.RadianToAngle + fOffsetAngle;
    // dr := dr + $ff;
    // dr := dr - floor(dr);
    // if dr > +0.5 then dr := dr - 1.0;
    // if dr < -0.5 then dr := dr + 1.0;
    Owner.aLocalAngle := dr;
  end
  else
  begin
    Owner.aLocalAngle := Owner.LocalAngle;
  end;

  TAnimrigMisc.m4PushRotateZ(im, -Owner.aLocalAngle);

  // position
  if not(LockBonePosition) then
  begin
    v2 := Child.WorldPosition;
    v2.Transform(im);
    Child.aLocalPosition := v2;
  end
  else
  begin
    Child.aLocalPosition := Child.LocalPosition;
  end;

  for p in Child.Springs do
    TPhysicsSpringBone(p).UpdateBoneLocal(im);
end;

{ TPhysicsGroupBone }

procedure TPhysicsGroupSpringBone.AddBone(b: TPhysicsNodeBone);
begin
  fNodes.Add(b);

  if b.Parent = nil then
  begin
    fRootBone := b;
  end;
end;

procedure TPhysicsGroupSpringBone.UpdateTransformForce;
var
  r1: double;
  v1, d: TPhysicsVector3;
  m, im: TPhysicsMatrix4;
begin
  if fRootBone = nil then
    exit;

  // optimize
  m := RootMatrix;
  TAnimrigMisc.m43PushMatrix(m, ExternalMatrix);

  v1 := fRootBone.LocalPosition;
  v1.Mul(ForceRotateRatio); // Value adjustment during rotation
  v1.Transform(m);
  r1 := 0.0;
  if fCacheInitFlag then
  begin
    fCacheRootPosition := v1;
    fCacheRootAngle := r1;
    fCacheInitFlag := false;
  end;

  if v1.Compare(fCacheRootPosition) and (r1 = fCacheRootAngle) then
    exit;

  // world position diff
  d.SetSub(v1, fCacheRootPosition);
  d.Mul(ForceRatio); // Value adjustment during movement

  // convert root local space (physics group space)
  im := InvMatrix2D(m);
  im._41 := 0.0; // disable move
  im._42 := 0.0;
  im._43 := 0.0;
  d.Transform(im);

  SlidSpring(d.x, d.y, fRootBone);

  fCacheRootPosition := v1;
  fCacheRootAngle := r1;
end;

procedure TPhysicsGroupSpringBone.AddForce(const d: TPhysicsVector3);
var
  m: TPhysicsMatrix4;
  v: TPhysicsVector3;
begin
  m := InvMatrix2D(RootMatrix);
  m._41 := 0.0; // disable move
  m._42 := 0.0;
  m._43 := 0.0;
  v := d;
  v.Transform(m);

  SlidSpring(-v.x, -v.y, fRootBone);
end;

procedure TPhysicsGroupSpringBone.AddSpring(s: TPhysicsSpringBone);
begin
  case Mode of
    gsmStrPoint:
      begin
        s.FixedLength := false;
        s.LockBoneAngle := true;
        s.LockBonePosition := false;
      end;
    gsmPoint:
      begin
        s.FixedLength := true;
        s.LockBoneAngle := true;
        s.LockBonePosition := false;
      end;
    gsmStrBone:
      begin
        s.FixedLength := false;
        s.LockBoneAngle := false;
        s.LockBonePosition := false;
      end;
    gsmBone:
      begin
        s.FixedLength := true;
        s.LockBoneAngle := false;
        s.LockBonePosition := true;
      end;
  end;

  fSprings.Add(s);
end;

procedure TPhysicsGroupSpringBone.Appease;
var
  p: pointer;
begin
  RootBone.UpdateWorldMatrix(TAnimrigMisc.IdentityMatrix, true);
  for p in RootBone.Springs do
    TPhysicsSpringBone(p).Appease;
end;

procedure TPhysicsGroupSpringBone.Apply;
var
  p: pointer;
begin
  for p in RootBone.Springs do
    TPhysicsSpringBone(p).UpdateBoneLocal(TAnimrigMisc.IdentityMatrix);
end;

constructor TPhysicsGroupSpringBone.Create;
begin
  inherited Create;

  fGroupType := gtSpring;
  fMode := gsmStrPoint;

  fNodes := TObjectList.Create;
  fSprings := TObjectList.Create;

  fRootBone := nil;

  ExternalMatrix := TAnimrigMisc.IdentityMatrix;
  RootMatrix := TAnimrigMisc.IdentityMatrix;

  RootUserObject := nil;

  ForceRotateRatio := 1.0;
  ForceRatio := 1.0;
  Alpha := 1.0;
  ForceWiggle := 0.0;

  fCacheInitFlag := true;
  fCacheRootPosition.SetVector(0, 0, 0);
  fCacheRootAngle := 0;
  ElapsedTime := random(1024);
end;

destructor TPhysicsGroupSpringBone.Destroy;
begin
  fNodes.Free;
  fSprings.Free;

  inherited;
end;

function TPhysicsGroupSpringBone.GetBone(n: integer): TPhysicsNodeBone;
begin
  result := nil;
  if n >= fNodes.Count then
    exit;
  result := TPhysicsNodeBone(fNodes[n]);
end;

function TPhysicsGroupSpringBone.GetSpring(n: integer): TPhysicsSpringBone;
begin
  result := nil;
  if n >= fSprings.Count then
    exit;
  result := TPhysicsSpringBone(fSprings[n]);
end;

function TPhysicsGroupSpringBone.InvMatrix2D(const m: TPhysicsMatrix4)
  : TPhysicsMatrix4;
var
  d: double;
begin
  result := TAnimrigMisc.IdentityMatrix;

  d := m._11 * m._22 - m._12 * m._21;
  if abs(d) < 0.000001 then
    d := 0.000001;

  d := 1.0 / d;
  result._11 := +m._22 * d;
  result._12 := -m._12 * d;
  result._21 := -m._21 * d;
  result._22 := +m._11 * d;

  result._41 := -m._41;
  result._42 := -m._42;
  result._43 := -m._43;
end;

procedure TPhysicsGroupSpringBone.SlidSpring(x, y: double;
  bone: TPhysicsNodeBone);
  procedure inSlide(x, y: double; bone: TPhysicsNodeBone);
  var
    p: pointer;
  begin
    if bone = nil then
      exit;

    if not(bone.Fixed) then
    begin
      bone.WorldPosition.x := bone.WorldPosition.x - x;
      bone.WorldPosition.y := bone.WorldPosition.y - y;
    end;

    for p in bone.Children do
      inSlide(x, y, TPhysicsNodeBone(p));
  end;

var
  w: single;
begin
  if bone = nil then
    exit;

  w := TAnimrigWiggle.random(ElapsedTime * 2,
    TAnimrigWiggle.WiggleType.anwLinear, 273) * ForceWiggle +
    (1.0 - ForceWiggle * 0.5);
  x := x * w;
  y := y * w;

  inSlide(x, y, bone);
end;

procedure TPhysicsGroupSpringBone.SetupNatural;
var
  p: pointer;
begin
  RootBone.UpdateWorldMatrix(TAnimrigMisc.IdentityMatrix, true); // build worldposition. but worldmatrix not used in SetupNatural.
  // build spring natual
  for p in RootBone.Springs do
    TPhysicsSpringBone(p).SetupNatural(TAnimrigMisc.IdentityMatrix);

  // clear force
  fCacheInitFlag := true;
end;

procedure TPhysicsGroupSpringBone.Update(dt: double);
var
  t: double;
  p: pointer;
begin
  ElapsedTime := ElapsedTime + dt;

  UpdateTransformForce;

  // safe
  K := min(max(K, 0.1), 100 * 100);
  Mass := min(max(Mass, 0.001), 100 * 100);
  Damping := min(max(Damping, 0.1), 100 * 100);
  Gravity := min(max(Gravity, 0.0), 100 * 100);
  if GravityVector.Length > 1.0 then
    GravityVector.Normalize;
  LimitVelocity := min(max(LimitVelocity, 1), $FFFF);

  // 100 fps
  while dt >= 0.0 do
  begin
    t := min(dt, 1.0 / 100.0);
    for p in RootBone.Springs do
      TPhysicsSpringBone(p).Update(t);
    dt := dt - (1.0 / 100.0);
  end;
end;

procedure TPhysicsGroupSpringBone.UpdateWorldMatrix;
begin
  RootBone.UpdateWorldMatrix(TAnimrigMisc.IdentityMatrix, false);
end;

{ TPhysicsGroup }

constructor TPhysicsGroup.Create;
begin
  Name := '';
  AliasName := '';
end;

{ TPhysicsSpringGroup }

constructor TPhysicsSpringGroup.Create;
begin
  inherited;

  K := 100.0;
  Damping := 10.0;
  Mass := 1.0;
  Gravity := 0.0;
  GravityVector.SetVector(0, -1, 0);
  LimitVelocity := 1000;

  LimitAngleMin := -$FF;
  LimitAngleMax := +$FF;
  LimitMoveRatio := $FFFF;
end;

{ TPhysicsSpring }

constructor TPhysicsSpring.Create(group: TPhysicsGroup);
begin
  fOwnerGroup := group;
end;

{ TPhysicsElementGroup }

constructor TPhysicsElementGroup.Create;
begin
  K := 100.0;
  Damping := 10.0;
  Mass := 1.0;
  LimitVelocity := 1000;
  // LimitAngleMin := -$ff;
  // LimitAngleMax := +$ff;
  // LimitMoveRatio := $FFFF;
end;

{ TPhysicsSpringElement }

procedure TPhysicsSpringElement.Appease;
begin
  aValue := Value;
  pValue := Value;
  fVelocity.SetVector(0,0,0);
  fStartupFlag := true;
end;

constructor TPhysicsSpringElement.Create(group: TPhysicsGroup);
begin
  inherited Create(group);

  Mask := 1 + 2 + 4;
  Alpha := 1.0;

  Value.SetVector(0, 0, 0);
  aValue.SetVector(0, 0, 0);
  pValue.SetVector(0, 0, 0);
  fStartupFlag := true;
end;

procedure TPhysicsSpringElement.SetNatural(v: TPhysicsVector3);
begin
  // unnecessary natural
end;

procedure TPhysicsSpringElement.Update(dt: double);
var
  d: TPhysicsVector3;
  force: double;
  accel: double;
  Mass, imass: double;
  K, Damping: double;
  sg: TPhysicsElementGroup;
begin
  //startup
  if fStartupFlag then
  begin
    Appease;  // copy input to applied
    fStartupFlag := false;
  end;

  if (Mask = 0) then
  begin
    aValue := Value;
    pValue := Value;
    fVelocity.SetVector(0,0,0);  // safety
    exit;
  end;

  sg := TPhysicsElementGroup(fOwnerGroup);
  Mass := max(sg.Mass, 0.001);
  imass := 1.0 / Mass;
  K := sg.K;

  d.SetSub(pValue,Value);

  force := -K * d.x;
  Damping := fVelocity.x * sg.Damping;
  force := force  - Damping;
  accel := force * imass;
  fVelocity.x := fVelocity.x + accel * dt;

  force := -K * d.y;
  Damping := fVelocity.y * sg.Damping;
  force := force - Damping;
  accel := force * imass;
  fVelocity.y := fVelocity.y + accel * dt;

  force := -K * d.z;
  Damping := fVelocity.z * sg.Damping;
  force := force - Damping;
  accel := force * imass;
  fVelocity.z := fVelocity.z + accel * dt;

  if fVelocity.Length > sg.LimitVelocity * 100 then // 100 = 100fps
  begin
    fVelocity.Normalize;
    fVelocity.Mul(sg.LimitVelocity);
  end;

  d := fVelocity;
  d.Mul(dt);
  pValue.SetAdd(pValue,d);
  if (Mask and 1 = 0) then pValue.x := Value.x;
  if (Mask and 2 = 0) then pValue.y := Value.y;
  if (Mask and 4 = 0) then pValue.z := Value.z;

  aValue := pValue;
  if (Alpha <> 1.0) then aValue.SetBlend(Value,aValue,Alpha);
end;

{ TPhysicsGroupElementBone }

procedure TPhysicsGroupElementBone.AddForce(const d: TPhysicsVector3);
begin
  // unsupport
end;

procedure TPhysicsGroupElementBone.Appease;
begin
  fPosition.Appease;
  fScale.Appease;
  fRotate.Appease;
end;

procedure TPhysicsGroupElementBone.Apply;
begin
  // Unnecessary
end;

constructor TPhysicsGroupElementBone.Create;
begin
  inherited Create;
  fGroupType := gtElement;

  fPosition := TPhysicsSpringElement.Create(self);
  fScale := TPhysicsSpringElement.Create(self);
  fRotate := TPhysicsSpringElement.Create(self);
end;

destructor TPhysicsGroupElementBone.Destroy;
begin
  fPosition.Free;
  fScale.Free;
  fRotate.Free;

  inherited;
end;

procedure TPhysicsGroupElementBone.Update(dt: double);
var
  t : double;
  d : TPhysicsVector3;
begin
  // rotation loop
  d.SetSub(fRotate.pValue,SpringRotate.Value);
  d.x := d.x - (16384 - floor(16384.499999999996 - d.x));    // convert to -0.5 ... +0.5
  d.y := d.y - (16384 - floor(16384.499999999996 - d.y));
  d.z := d.z - (16384 - floor(16384.499999999996 - d.z));
  SpringRotate.pValue.SetAdd(SpringRotate.Value,d);

  // 100 fps
  while dt >= 0.0 do
  begin
    t := min(dt, 1.0 / 100.0);
    fPosition.Update(t);
    fScale.Update(t);
    fRotate.Update(t);

    dt := dt - (1.0 / 100.0);
  end;
end;

end.
