hgephysics.pas

完整的Delphi游戏开发控件

    VOut[Result] := VIn[0];
    Inc(Result);
  end;

  if (Distance1 <= 0) then begin
    VOut[Result] := VIn[1];
    Inc(Result);
  end;

	// If the points are on different sides of the plane
  if (Distance0 * Distance1 < 0) then begin
		// Find intersection point of edge and plane
    Interp := Distance0 / (Distance0 - Distance1);
    VOut[Result].V := VIn[0].V + Interp * (VIn[1].V - VIn[0].V);
    if (Distance0 > 0) then begin
      VOut[Result].FP := VIn[0].FP;
      VOut[Result].FP.E.InEdge1 := ClipEdge;
      VOut[Result].FP.E.InEdge2 := NoEdge;
    end else begin
      VOut[Result].FP := VIn[1].FP;
      VOut[Result].FP.E.OutEdge1 := ClipEdge;
      VOut[Result].FP.E.OutEdge2 := NoEdge;
    end;
    Inc(Result);
  end;
end;

procedure Flip(var FP: TFeaturePair);
begin
  Swap(FP.E.InEdge1,FP.E.InEdge2);
  Swap(FP.E.OutEdge1,FP.E.OutEdge2);
end;

// The normal points from A to B
function Collide(var Contacts: TContacts; const BodyA, BodyB: IHGEBody): Integer;
const
  RelativeTol = 0.95;
	AbsoluteTol = 0.01;
var
  hA, hB, PosA, PosB, a1, a2, b1, b2, dp, dA, dB, FaceA, FaceB, Normal: THGEVector;
  FrontNormal, SideNormal: THGEVector;
  RotA, RotB, RotAT, RotBT, C, AbsC, AbsCT: THGEMatrix;
  Axis: TAxis;
  Separation, Front, NegSide, PosSide, Side: Single;
  IncidentEdge, ClipPoints1, ClipPoints2: TClipVertices;
  NegEdge, PosEdge: Byte;
  NP, I: Integer;
begin
  Result := 0;

	// Setup
  hA := 0.5 * BodyA.Size;
  hB := 0.5 * BodyB.Size;

  PosA := BodyA.Position;
  PosB := BodyB.Position;

  RotA := THGEMatrix.Create(BodyA.Rotation);
  RotB := THGEMatrix.Create(BodyB.Rotation);

  RotAT := RotA.Transpose;
  RotBT := RotB.Transpose;

  a1 := RotA.Col1;
  a2 := RotA.Col2;
  b1 := RotB.Col1;
  b2 := RotB.Col2;

  dp := PosB - PosA;
  dA := RotAT * dp;
  dB := RotBT * dp;

  C := RotAT * RotB;
  AbsC := C.Abs;
  AbsCT := AbsC.Transpose;

	// Box A faces
  FaceA := dA.Abs - hA - AbsC * hB;
  if (FaceA.X > 0) or (FaceA.Y > 0) then
    Exit;

	// Box B faces
  FaceB := dB.Abs - AbsCT * hA - hB;
  if (FaceB.X > 0) or (FaceB.Y > 0) then
    Exit;

	// Find best axis

	// Box A faces
  Axis := FaceAX;
  Separation := FaceA.X;
  if (dA.X > 0) then
    Normal := RotA.Col1
  else
    Normal := -RotA.Col1;

  if (FaceA.Y > RelativeTol * Separation + AbsoluteTol * hA.Y) then begin
    Axis := FaceAY;
    Separation := FaceA.Y;
    if (dA.Y > 0) then
      Normal := RotA.Col2
    else
      Normal := -RotA.Col2;
  end;

	// Box B faces
  if (FaceB.X > RelativeTol * Separation + AbsoluteTol * hB.X) then begin
    Axis := FaceBX;
    Separation := FaceB.X;
    if (dB.X > 0) then
      Normal := RotB.Col1
    else
      Normal := -RotB.Col1;
  end;

  if (FaceB.Y > RelativeTol * Separation + AbsoluteTol * hB.Y) then begin
    Axis := FaceBY;
//    Separation := FaceB.Y;
    if (dB.Y > 0) then
      Normal := RotB.Col2
    else
      Normal := -RotB.Col2;
  end;

	// Setup clipping plane data based on the separating axis

	// Compute the clipping lines and the line segment to be clipped.
  case Axis of
    FaceAX:
      begin
        FrontNormal := Normal;
        Front := PosA.Dot(FrontNormal) + hA.X;
        SideNormal := RotA.Col2;
        Side := PosA.Dot(SideNormal);
        NegSide := -Side + hA.Y;
        PosSide :=  Side + hA.Y;
        NegEdge := Edge3;
        PosEdge := Edge1;
        ComputeIncidentEdge(IncidentEdge,hB,PosB,RotB,FrontNormal);
      end;
    FaceAY:
      begin
        FrontNormal := Normal;
        Front := PosA.Dot(FrontNormal) + hA.Y;
        SideNormal := RotA.Col1;
        Side := PosA.Dot(SideNormal);
        NegSide := -Side + hA.X;
        PosSide :=  Side + hA.X;
        NegEdge := Edge2;
        PosEdge := Edge4;
        ComputeIncidentEdge(IncidentEdge,hB,PosB,RotB,FrontNormal);
      end;
    FaceBX:
      begin
        FrontNormal := -Normal;
        Front := PosB.Dot(FrontNormal) + hB.X;
        SideNormal := RotB.Col2;
        Side := PosB.Dot(SideNormal);
        NegSide := -Side + hB.Y;
        PosSide :=  Side + hB.Y;
        NegEdge := Edge3;
        PosEdge := Edge1;
        ComputeIncidentEdge(IncidentEdge,hA,PosA,RotA,FrontNormal);
      end;
  else
    // FaceBY:
      begin
        FrontNormal := -Normal;
        Front := PosB.Dot(FrontNormal) + hB.Y;
        SideNormal := RotB.Col1;
        Side := PosB.Dot(SideNormal);
        NegSide := -Side + hB.X;
        PosSide :=  Side + hB.X;
        NegEdge := Edge2;
        PosEdge := Edge4;
        ComputeIncidentEdge(IncidentEdge,hA,PosA,RotA,FrontNormal);
      end;
  end;

	// clip other face with 5 box planes (1 face plane, 4 edge planes)

	// Clip to box side 1
  NP := ClipSegmentToLine(ClipPoints1,IncidentEdge,-SideNormal,NegSide,NegEdge);
  if (NP < 2) then
    Exit;

	// Clip to negative box side 1
  NP := ClipSegmentToLine(ClipPoints2,ClipPoints1,SideNormal,PosSide,PosEdge);
  if (NP < 2) then
    Exit;

	// Now clipPoints2 contains the clipping points.
	// Due to roundoff, it is possible that clipping removes all points.
  for I := 0 to 1 do begin
    Separation := FrontNormal.Dot(ClipPoints2[I].V) - Front;
    if (Separation <= 0) then begin
      Contacts[Result].Separation := Separation;
      Contacts[Result].Normal := Normal;
			// slide contact point onto reference face (easy to cull)
      Contacts[Result].Position := ClipPoints2[I].V - Separation * FrontNormal;
      Contacts[Result].Feature := ClipPoints2[I].FP;
      if (Axis in [FaceBX,FaceBY]) then
        Flip(Contacts[Result].Feature);
      Inc(Result);
    end;
  end;
end;

{ THGEBody }

procedure THGEBody.AddForce(const Force: THGEVector);
begin
  FForce := FForce + Force;
end;

constructor THGEBody.Create(const ASize: THGEVector; const AMass: Single);
begin
  inherited Create;
  Inc(HGEBodyCount);
  FFriction := 0.2;
  FSize := ASize;
  FMass := AMass;

  if (FMass < MaxSingle - 1e31) then begin
    FInvMass := 1 / FMass;
    FI := FMass * ((FSize.X * FSize.X) + (FSize.Y * FSize.Y)) / 12;
    FInvI := 1 / FI;
  end else begin
    FInvMass := 0;
    FI := MaxSingle;
    FInvI := 0;
  end;
end;

destructor THGEBody.Destroy;
begin
  Dec(HGEBodyCount);
  inherited;
end;

constructor THGEBody.Create(const ASize: THGEVector);
begin
  Create(ASize,MaxSingle);
end;

constructor THGEBody.Create;
begin
  Create(THGEVector.Create(1,1),MaxSingle);
end;

function THGEBody.GetAngularVelocity: Single;
begin
  Result := FAngularVelocity;
end;

function THGEBody.GetBiasedAngularVelocity: Single;
begin
  Result := FBiasedAngularVelocity;
end;

function THGEBody.GetBiasedVelocity: THGEVector;
begin
  Result := FBiasedVelocity;
end;

function THGEBody.GetForce: THGEVector;
begin
  Result := FForce;
end;

function THGEBody.GetFriction: Single;
begin
  Result := FFriction;
end;

function THGEBody.GetI: Single;
begin
  Result := FI;
end;

function THGEBody.GetInvI: Single;
begin
  Result := FInvI;
end;

function THGEBody.GetInvMass: Single;
begin
  Result := FInvMass;
end;

function THGEBody.GetIsStationary: Boolean;
begin
  Result := (FMass > MaxSingle - 1e31);
end;

function THGEBody.GetMass: Single;
begin
  Result := FMass;
end;

function THGEBody.GetPBiasedVelocity: PHGEVector;
begin
  Result := @FBiasedVelocity;
end;

function THGEBody.GetPForce: PHGEVector;
begin
  Result := @FForce;
end;

function THGEBody.GetPosition: THGEVector;
begin
  Result := FPosition;
end;

function THGEBody.GetPPosition: PHGEVector;
begin
  Result := @FPosition;
end;

function THGEBody.GetPSize: PHGEVector;
begin
  Result := @FSize;
end;

function THGEBody.GetPVelocity: PHGEVector;
begin
  Result := @FVelocity;
end;

function THGEBody.GetRotation: Single;
begin
  Result := FRotation;
end;

function THGEBody.GetSize: THGEVector;
begin
  Result := FSize;
end;

function THGEBody.GetTorque: Single;
begin
  Result := FTorque;
end;

function THGEBody.GetVelocity: THGEVector;
begin
  Result := FVelocity;
end;

procedure THGEBody.SetAngularVelocity(const Value: Single);
begin
  FAngularVelocity := Value;
end;

procedure THGEBody.SetBiasedAngularVelocity(const Value: Single);
begin
  FBiasedAngularVelocity := Value;
end;

procedure THGEBody.SetBiasedVelocity(const Value: THGEVector);
begin
  FBiasedVelocity := Value;
end;

procedure THGEBody.SetForce(const Value: THGEVector);
begin
  FForce := Value;
end;

procedure THGEBody.SetFriction(const Value: Single);
begin
  FFriction := Value;
end;

procedure THGEBody.SetPosition(const Value: THGEVector);
begin
  FPosition := Value;
end;

procedure THGEBody.SetRotation(const Value: Single);
begin
  FRotation := Value;
end;

procedure THGEBody.SetTorque(const Value: Single);
begin
  FTorque := Value;
end;

procedure THGEBody.SetVelocity(const Value: THGEVector);
begin
  FVelocity := Value;
end;

{ THGEBodyList }

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

procedure THGEBodyList.Clear;
begin
  FBodies.Clear;
end;

constructor THGEBodyList.Create;
begin
  inherited;
  FBodies := TInterfaceList.Create;
end;

function THGEBodyList.GetBody(const Index: Integer): IHGEBody;
begin
  Result := IHGEBody(FBodies[Index]);
end;

function THGEBodyList.GetCount: Integer;
begin
  Result := FBodies.Count;
end;

{ THGEJoint }

procedure THGEJoint.ApplyImpulse;
var
  DV, Impulse: THGEVector;
begin
  DV := FBody2.Velocity + Cross(FBody2.AngularVelocity,FR2) - FBody1.Velocity
    - Cross(FBody1.AngularVelocity,FR1);
  Impulse := FM * (FBias - DV - FSoftness * FP);

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

  FBody2.PVelocity.Increment(FBody2.InvMass * Impulse);
  FBody2.AngularVelocity := FBody2.AngularVelocity + (FBody2.InvI * Cross(FR2,Impulse));

  FP.Increment(Impulse);
end;

constructor THGEJoint.Create;
begin
  inherited;
  Inc(HGEJointCount);
  FBiasFactor := 0.2;
end;

constructor THGEJoint.Create(const ABody1, ABody2: IHGEBody;
  const AAnchor: THGEVector);
var
  Rot1, Rot2, Rot1T, Rot2T: THGEMatrix;
begin
  inherited Create;
  Inc(HGEJointCount);
  FBody1 := ABody1;
  FBody2 := ABody2;

  Rot1 := THGEMatrix.Create(FBody1.Rotation);
  Rot2 := THGEMatrix.Create(FBody2.Rotation);
  Rot1T := Rot1.Transpose;
  Rot2T := Rot2.Transpose;

  FLocalAnchor1 := Rot1T * (AAnchor - FBody1.Position);
  FLocalAnchor2 := Rot2T * (AAnchor - FBody2.Position);

  FBiasFactor := 0.2;
end;

destructor THGEJoint.Destroy;
begin
  Dec(HGEJointCount);
  inherited;
end;

function THGEJoint.GetBiasFactor: Single;
begin
  Result := FBiasFactor;
end;

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

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

function THGEJoint.GetLocalAnchor1: THGEVector;
begin
  Result := FLocalAnchor1;
end;

function THGEJoint.GetLocalAnchor2: THGEVector;
begin
  Result := FLocalAnchor2;
end;

function THGEJoint.GetSoftness: Single;
begin
  Result := FSoftness;
end;

procedure THGEJoint.PreStep(const InvDt: Single);
var
  Rot1, Rot2, K1, K2, K3, K: THGEMatrix;
  P1, P2, DP: THGEVector;
begin
	// Pre-compute anchors, mass matrix, and bias.
  Rot1 := THGEMatrix.Create(FBody1.Rotation);
  Rot2 := THGEMatrix.Create(FBody2.Rotation);

  FR1 := Rot1 * FLocalAnchor1;
  FR2 := Rot2 * FLocalAnchor2;

	// deltaV = deltaV0 + K * impulse
	// invM = [(1/m1 + 1/m2) * eye(2) - skew(FR1) * invI1 * skew(FR1) - skew(FR2) * invI2 * skew(FR2)]
	//      = [1/m1+1/m2     0    ] + invI1 * [FR1.y*FR1.y -FR1.x*FR1.y] + invI2 * [FR1.y*FR1.y -FR1.x*FR1.y]
	//        [    0     1/m1+1/m2]           [-FR1.x*FR1.y FR1.x*FR1.x]           [-FR1.x*FR1.y FR1.x*FR1.x]
  K1.Col1.X := FBody1.InvMass + FBody2.InvMass;
  K1.Col2.X := 0;
  K1.Col1.Y := 0;
  K1.Col2.Y := FBody1.InvMass + FBody2.InvMass;

  K2.Col1.X :=  FBody1.InvI * FR1.Y * FR1.Y;
  K2.Col2.X := -FBody1.InvI * FR1.X * FR1.Y;
  K2.Col1.Y := -FBody1.InvI * FR1.X * FR1.Y;
  K2.Col2.Y :=  FBody1.InvI * FR1.X * FR1.X;

  K3.Col1.X :=  FBody2.InvI * FR2.Y * FR2.Y;
  K3.Col2.X := -FBody2.InvI * FR2.X * FR2.Y;
  K3.Col1.Y := -FBody2.InvI * FR2.X * FR2.Y;
  K3.Col2.Y :=  FBody2.InvI * FR2.X * FR2.X;

  K := K1 + K2 + K3;
  K.Col1.X := K.Col1.X + FSoftness;
  K.Col1.Y := K.Col1.Y + FSoftness;

  FM := K.Invert;

  P1 := FBody1.Position + FR1;
  P2 := FBody2.Position + FR2;
  DP := P2 - P1;

  if (THGEWorld.PositionCorrection) then
    FBias := -FBiasFactor * InvDt * DP
  else
    FBias := THGEVector.Create(0,0);

  if (THGEWorld.WarmStarting) then begin
		// Apply accumulated impulse.
    FBody1.PVelocity.Decrement(FBody1.InvMass * FP);
    FBody1.AngularVelocity := FBody1.AngularVelocity - (FBody1.InvI * Cross(FR1,FP));

    FBody2.PVelocity.Increment(FBody2.InvMass * FP);
    FBody2.AngularVelocity := FBody2.AngularVelocity + (FBody2.InvI * Cross(FR2,FP));
  end else
    FP := THGEVector.Create(0,0);
end;

procedure THGEJoint.SetBiasFactor(const Value: Single);
begin
  FBiasFactor := Value;