hgephysics.pas

完整的Delphi游戏开发控件

end;

procedure THGEJoint.SetSoftness(const Value: Single);
begin
  FSoftness := Value;
end;

{ THGEJointList }

procedure THGEJointList.Add(const Joint: IHGEJoint);
begin
  FJoints.Add(Joint);
end;

procedure THGEJointList.Clear;
begin
  FJoints.Clear;
end;

constructor THGEJointList.Create;
begin
  inherited;
  FJoints := TInterfaceList.Create;
end;

function THGEJointList.GetCount: Integer;
begin
  Result := FJoints.Count;
end;

function THGEJointList.GetJoint(const Index: Integer): IHGEJoint;
begin
  Result := IHGEJoint(FJoints[Index]);
end;

{ THGEArbiter }

procedure THGEArbiter.ApplyImpulse;
var
  I: Integer;
  C: PContact;
  DV, Pn, Pb, Tangent, Pt: THGEVector;
  VN, dPn, Pn0, VNB, dPnb, Pnb0, VT, dPt, MaxPt, OldTangentImpulse: Single;
begin
  for I := 0 to FNumContacts - 1 do begin
    C := @FContacts[I];
    C.R1 := C.Position - FBody1.Position;
    C.R2 := C.Position - FBody2.Position;

		// Relative velocity at contact
    DV := FBody2.Velocity + Cross(FBody2.AngularVelocity,C.R2) - FBody1.Velocity
      - Cross(FBody1.AngularVelocity,C.R1);

		// Compute normal impulse
    VN := DV.Dot(C.Normal);
    if (THGEWorld.SplitImpules) then
      dPn := C.MassNormal * (-VN)
    else
      dPn := C.MassNormal * (-VN + C.Bias);

    if (THGEWorld.AccumulateImpulses) then begin
			// Clamp the accumulated impulse
      Pn0 := C.Pn;
      C.Pn := Max(Pn0 + dPn,0);
      dPn := C.Pn - Pn0;
    end else
      dPn := Max(dPn,0);

		// Apply contact impulse
    Pn := dPn * C.Normal;

    FBody1.PVelocity.Decrement(FBody1.InvMass * Pn);
    FBody1.AngularVelocity := FBody1.AngularVelocity - (FBody1.InvI * Cross(C.R1,Pn));

    FBody2.PVelocity.Increment(FBody2.InvMass * Pn);
    FBody2.AngularVelocity := FBody2.AngularVelocity + (FBody2.InvI * Cross(C.R2,Pn));

    if (THGEWorld.SplitImpules) then begin
			// Compute bias impulse
      DV := FBody2.BiasedVelocity + Cross(FBody2.BiasedAngularVelocity,C.R2)
        - FBody1.BiasedVelocity - Cross(FBody1.BiasedAngularVelocity,C.R1);
      VNB := DV.Dot(C.Normal);

      dPnb := C.MassNormal * (-VNB + C.Bias);
      Pnb0 := C.Pnb;
      C.Pnb := Max(Pnb0 + dPnb,0);
      dPnb := C.Pnb - Pnb0;

      Pb := dPnb * C.Normal;

      FBody1.PBiasedVelocity.Decrement(FBody1.InvMass * Pb);
      FBody1.BiasedAngularVelocity := FBody1.BiasedAngularVelocity - (FBody1.InvI * Cross(C.R1,Pb));

      FBody2.PBiasedVelocity.Increment(FBody2.InvMass * Pb);
      FBody2.BiasedAngularVelocity := FBody2.BiasedAngularVelocity + (FBody2.InvI * Cross(C.R2,Pb));
    end;

		// Relative velocity at contact
    DV := FBody2.Velocity + Cross(FBody2.AngularVelocity,C.R2) - FBody1.Velocity
      - Cross(FBody1.AngularVelocity,C.R1);
    Tangent := Cross(C.Normal,1);
    VT := DV.Dot(Tangent);
    dPt := C.MassTangent * (-VT);

    if (THGEWorld.AccumulateImpulses) then begin
			// Compute friction impulse
      MaxPt := FFriction * C.Pn;

			// Clamp friction
      OldTangentImpulse := C.Pt;
      C.Pt := Clamp(OldTangentImpulse + dPt,-MaxPt,MaxPt);
      dPt := C.Pt - OldTangentImpulse;
    end else begin
      MaxPt := FFriction * dPn;
      dPt := Clamp(dPt,-MaxPt,MaxPt);
    end;

		// Apply contact impulse
    Pt := dPt * Tangent;

    FBody1.PVelocity.Decrement(FBody1.InvMass * Pt);
    FBody1.AngularVelocity := FBody1.AngularVelocity - (FBody1.InvI * Cross(C.R1,Pt));

    FBody2.PVelocity.Increment(FBody2.InvMass * Pt);
    FBody2.AngularVelocity := FBody2.AngularVelocity + (FBody2.InvI * Cross(C.R2,Pt));
  end;
end;

constructor THGEArbiter.Create(const ABody1, ABody2: IHGEBody;
  const AContacts: TContacts; const ANumContacts: Integer);
var
  I: Integer;
begin
  inherited Create;
  Inc(HGEArbiterCount);
  if (Cardinal(ABody1) < Cardinal(ABody2)) then begin
    FBody1 := ABody1;
    FBody2 := ABody2;
  end else begin
    FBody1 := ABody2;
    FBody2 := ABody1;
  end;
  FNumContacts := ANumContacts;
  for I := 0 to FNumContacts - 1 do
    FContacts[I] := AContacts[I];
  FFriction := Sqrt(FBody1.Friction * FBody2.Friction);
end;

destructor THGEArbiter.Destroy;
begin
  FBody1 := nil;
  FBody2 := nil;
  FNext := nil;
  Dec(HGEArbiterCount);
  inherited;
end;

function THGEArbiter.GetBody1: IHGEBody;
begin
  Result := FBody1;
end;

function THGEArbiter.GetBody2: IHGEBody;
begin
  Result := FBody2;
end;

function THGEArbiter.GetNext: IHGEArbiter;
begin
  Result := FNext;
end;

function THGEArbiter.Implementor: TObject;
begin
  Result := Self;
end;

procedure THGEArbiter.PreStep(const InvDT: Single);
const
  AllowedPenetration = 0.01;
var
  BiasFactor, RN1, RN2, kNormal, RT1, RT2, kTangent: Single;
  I: Integer;
  C: PContact;
  R1, R2, Tangent, P: THGEVector;
begin
  if (THGEWorld.PositionCorrection) then
    if (THGEWorld.SplitImpules) then
      BiasFactor := 0.8
    else
      BiasFactor := 0.2
  else
    BiasFactor := 0;

  for I := 0 to FNumContacts - 1 do begin
    C := @FContacts[I];

    R1 := C.Position - FBody1.Position;
    R2 := C.Position - FBody2.Position;

		// Precompute normal mass, tangent mass, and bias.
    RN1 := R1.Dot(C.Normal);
    RN2 := R2.Dot(C.Normal);
    kNormal := FBody1.InvMass + FBody2.InvMass;
    kNormal := kNormal + (FBody1.InvI * (R1.Dot(R1) - RN1 * RN1)
      + FBody2.InvI * (R2.Dot(R2) - RN2 * RN2));
    C.MassNormal := 1 / kNormal;

    Tangent := Cross(C.Normal,1);
    RT1 := R1.Dot(Tangent);
    RT2 := R2.Dot(Tangent);
    kTangent := FBody1.InvMass + FBody2.InvMass;
    kTangent := kTangent + (FBody1.InvI * (R1.Dot(R1) - RT1 * RT1)
      + FBody2.InvI * (R2.Dot(R2) - RT2 * RT2));
    C.MassTangent := 1 / kTangent;

    C.Bias := -BiasFactor * InvDT * Min(0,C.Separation + AllowedPenetration);

    if (THGEWorld.AccumulateImpulses) then begin
			// Apply normal + friction impulse
      P := C.Pn * C.Normal + C.Pt * Tangent;

      FBody1.PVelocity.Decrement(FBody1.InvMass * P);
      FBody1.AngularVelocity := FBody1.AngularVelocity - (FBody1.InvI * Cross(R1,P));

      FBody2.PVelocity.Increment(FBody2.InvMass * P);
      FBody2.AngularVelocity := FBody2.AngularVelocity + (FBody2.InvI * Cross(R2,P));
    end;
  end;
end;

procedure THGEArbiter.SetNext(const Value: IHGEArbiter);
begin
  FNext := Value;
end;

procedure THGEArbiter.Update(const NewContacts: TContacts;
  const NewNumContacts: Integer);
var
  MergedContacts: TContacts;
  I, J, K: Integer;
  CNew, COld, C: PContact;
begin
  for I := 0 to NewNumContacts - 1 do begin
    CNew := @NewContacts[I];
    K := -1;
    for J := 0 to FNumContacts - 1 do begin
      COld := @FContacts[J];
      if (CNew.Feature.Value = COld.Feature.Value) then begin
        K := J;
        Break;
      end;
    end;

    if (K > -1) then begin
      C := @MergedContacts[I];
      COld := @FContacts[K];
      C^ := CNew^;
      if (THGEWorld.WarmStarting) then begin
        C.Pn := COld.Pn;
        C.Pt := COld.Pt;
        C.Pnb := COld.Pnb;
      end else begin
        C.Pn := 0;
        C.Pt := 0;
        C.Pnb := 0;
      end;
    end else
      MergedContacts[I] := NewContacts[I];
  end;

  for I := 0 to NewNumContacts - 1 do
    FContacts[I] := MergedContacts[I];

  FNumContacts := NewNumContacts;
end;

{ THGEArbiterList }

procedure THGEArbiterList.Add(const Arbiter: IHGEArbiter);
var
  HashCode: Cardinal;
begin
  HashCode := Hash(Arbiter.Body1,Arbiter.Body2);
  Arbiter.Next := FBuckets[HashCode];
  FBuckets[HashCode] := Arbiter;
  Inc(FCount);
end;

procedure THGEArbiterList.Clear;
var
  I: Integer;
  Entry, Next: IHGEArbiter;
begin
  FCurrentEntry := nil;
  for I := 0 to ArbiterHashSize - 1 do begin
    Entry := FBuckets[I];
    if Assigned(Entry) then begin
      FBuckets[I] := nil;
      while Assigned(Entry) do begin
        Next := Entry.Next;
        Entry.Next := nil;
        Entry := Next;
      end;
    end;
  end;
  FCount := 0;
  FCurrentIndex := -1;
end;

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

procedure THGEArbiterList.Erase(const Body1, Body2: IHGEBody);
var
  HashCode: Cardinal;
  B1, B2: IHGEBody;
  Entry, Prev: IHGEArbiter;
begin
  if (Cardinal(Body1) < Cardinal(Body2)) then begin
    B1 := Body1;
    B2 := Body2;
  end else begin
    B1 := Body2;
    B2 := Body1;
  end;

  HashCode := Hash(Body1,Body2);
  Prev := nil;
  Entry := FBuckets[HashCode];

  while Assigned(Entry) and ((Entry.Body1 <> B1) or (Entry.Body2 <> B2)) do begin
    Prev := Entry;
    Entry := Entry.Next;
  end;

  if Assigned(Entry) then begin
    if Assigned(Prev) then
      Prev.Next := Entry.Next
    else
      FBuckets[HashCode] := Entry.Next;
    Dec(FCount);
  end;
end;

function THGEArbiterList.Find(const Body1, Body2: IHGEBody): IHGEArbiter;
var
  HashCode: Cardinal;
  B1, B2: IHGEBody;
begin
  if (Cardinal(Body1) < Cardinal(Body2)) then begin
    B1 := Body1;
    B2 := Body2;
  end else begin
    B1 := Body2;
    B2 := Body1;
  end;

  HashCode := Hash(Body1,Body2);
  Result := FBuckets[HashCode];

  while Assigned(Result) and ((Result.Body1 <> B1) or (Result.Body2 <> B2)) do
    Result := Result.Next;
end;

function THGEArbiterList.First: IHGEArbiter;
begin
  FCurrentIndex := -1;
  FCurrentEntry := nil;
  Result := Next;
end;

function THGEArbiterList.GetCount: Integer;
begin
  Result := FCount;
end;

function THGEArbiterList.Hash(const Body1, Body2: IHGEBody): Cardinal;
begin
  Result := ((Cardinal(Body1) + Cardinal(Body2)) shr 4) and ArbiterHashMask;
end;

function THGEArbiterList.Next: IHGEArbiter;
begin
  if Assigned(FCurrentEntry) then
    Result := FCurrentEntry.Next
  else
    Result := nil;

  while (Result = nil) and (FCurrentIndex < ArbiterHashSize - 1) do begin
    Inc(FCurrentIndex);
    Result := FBuckets[FCurrentIndex];
  end;

  FCurrentEntry := Result;
end;

{ THGEWorld }

procedure THGEWorld.Add(const Joint: IHGEJoint);
begin
  FJoints.Add(Joint);
end;

procedure THGEWorld.BroadPhase;
var
  I, J, NumContacts: Integer;
  BI, BJ: IHGEBody;
  Arb: IHGEArbiter;
  Contacts: TContacts;
begin
	// O(n^2) broad-phase
  FillChar(Contacts,SizeOf(Contacts),0);
  for I := 0 to FBodies.Count - 1 do begin
    BI := FBodies[I];
    for J := I + 1 to FBodies.Count - 1 do begin
      BJ := FBodies[J];
      if (BI.InvMass <> 0) or (BJ.InvMass <> 0) then begin
        { Collide calculation depends on order of bodies as used by arbiter }
        if (Cardinal(BI) < Cardinal(BJ)) then
          NumContacts := Collide(Contacts,BI,BJ)
        else
          NumContacts := Collide(Contacts,BJ,BI);
        if (NumContacts > 0) then begin
          Arb := FArbiters.Find(BI,BJ);
          if Assigned(Arb) then
            THGEArbiter(Arb.Implementor).Update(Contacts,NumContacts)
          else begin
            Arb := THGEArbiter.Create(BI,BJ,Contacts,NumContacts);
            FArbiters.Add(Arb);
          end;
        end else
          FArbiters.Erase(BI,BJ);
      end;
    end;
  end;
end;

procedure THGEWorld.Add(const Body: IHGEBody);
begin
  FBodies.Add(Body);
end;

procedure THGEWorld.Clear;
begin
  FArbiters.Clear;
  FJoints.Clear;
  FBodies.Clear;
end;

constructor THGEWorld.Create(const AGravity: THGEVector;
  const AIterations: Integer);
begin
  inherited Create;
  FGravity := AGravity;
  FIterations := AIterations;
  FBodies := THGEBodyList.Create;
  FJoints := THGEJointList.Create;
  FArbiters := THGEArbiterList.Create;
end;

destructor THGEWorld.Destroy;
begin
  Clear;
  FArbiters := nil;
  FJoints := nil;
  FBodies := nil;
  inherited;
end;

function THGEWorld.GetArbiters: IHGEArbiterList;
begin
  Result := FArbiters;
end;

function THGEWorld.GetBodies: IHGEBodyList;
begin
  Result := FBodies;
end;

function THGEWorld.GetGravity: THGEVector;
begin
  Result := FGravity;
end;

function THGEWorld.GetIterations: Integer;
begin
  Result := FIterations;
end;

function THGEWorld.GetJoints: IHGEJointList;
begin
  Result := FJoints;
end;

procedure THGEWorld.Step(const DT: Single);
var
  InvDT: Single;
  I, J: Integer;
  B: IHGEBody;
  A: IHGEArbiter;
begin
  if (DT > 0) then
    InvDT := 1 / DT
  else
    InvDT := 0;

	// Determine overlapping bodies and update contact points.
  BroadPhase;

	// Integrate forces.
  for I := 0 to FBodies.Count - 1 do begin
    B := FBodies[I];
    if (B.InvMass <> 0) then begin
      B.PVelocity.Increment(DT * (FGravity + B.InvMass * B.Force));
      B.AngularVelocity := B.AngularVelocity + (DT * B.InvI * B.Torque);
  		// Bias velocities are reset to zero each step.
      B.BiasedVelocity := THGEVector.Create(0,0);
      B.BiasedAngularVelocity := 0;
    end;
  end;

	// Perform pre-steps.
  A := FArbiters.First;
  while Assigned(A) do begin
    A.PreStep(InvDT);
    A := FArbiters.Next;
  end;

  for I := 0 to FJoints.Count - 1 do
    FJoints[I].PreStep(InvDT);

	// Perform iterations
  for I := 0 to FIterations - 1 do begin
    A := FArbiters.First;
    while Assigned(A) do begin
      A.ApplyImpulse;
      A := FArbiters.Next;
    end;

    for J := 0 to FJoints.Count - 1 do
      FJoints[J].ApplyImpulse;
  end;

	// Integrate Velocities
  for I := 0 to FBodies.Count - 1 do begin
    B := FBodies[I];
    B.PPosition.Increment(DT * (B.Velocity + B.BiasedVelocity));
    B.Rotation := B.Rotation + (DT * (B.AngularVelocity + B.BiasedAngularVelocity));

    B.Force := THGEVector.Create(0,0);
    B.Torque := 0;
  end;
end;

initialization
  THGEWorld.AccumulateImpulses := True;
  THGEWorld.SplitImpules := True;
  THGEWorld.WarmStarting := True;
  THGEWorld.PositionCorrection := True;

end.