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 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,ntAngle);
  TPhysicsSpringModel = (smOneSide,smBoth);
  TPhysicsGroupType = (gtSpring);
  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;
  TPhysicsSpringGroup = class(TPhysicsGroup)
  public
    K : double;
    Damping : double;
    Gravity : double;
    Mass : double;
    GravityVector : TPhysicsVector3;
    LimitVelocity : double;
    LimitAngleMin,LimitAngleMax : double;
    LimitMoveRatio : double;

    constructor Create;
  end;
  // basic spring
  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;
  // 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;

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;

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.m4PushMatrix(WorldMatrix,parentmatrix);

    v.SetVector(0,0,0);
    v.Transform(WorldMatrix);
    WorldPosition := v;
  end
  else
  begin
    WorldMatrix := GetAppliedLocalMatrix;
    TAnimrigMisc.m4PushMatrix(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.m4PushMatrix(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.m4PushMatrix(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;

end.
