newtoncustomjoints.pas

Newton Game Dynamic 1.52 Delphi下基于GLScene的OpenGL游戏开发控件。功能非常强大和易于使用。 Advanced physics engine for re

FLimitsOn := False;
FMinDist  := -1.0;
FMaxDist  := 1.0;

// calculate the two local matrix of the pivot point
CalculateLocalMatrix(aPivot, aPin, FLocalMatrix0, FLocalMatrix1);
end;

procedure TNewtonCustomJointSlider.EnableLimits(aEnabled: Boolean);
Begin
FLimitsOn := aEnabled;
end;

procedure TNewtonCustomJointSlider.SetLimits(aMinDist, aMaxDist: Single);
Begin
FMinDist := aMinDist;
FMaxDist := aMaxDist;
end;

procedure TNewtonCustomJointSlider.SubmitConstraint;
var
 LDist    : Single;
 LMatrix0 : TMatrix4f;
 LMatrix1 : TMatrix4f;
 LP0      : TVector4f;
 LP1      : TVector4f;
 LQ0      : TVector4f;
 LQ1      : TVector4f;
 LR0      : TVector4f;
 LR1      : TVector4f;
begin
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(FLocalMatrix0, FLocalMatrix1, LMatrix0, LMatrix1);

// Restrict the movement on the pivot point along all tree orthonormal direction
LP0 := V4(LMatrix0,3);
LP1 := VAdd(V4(LMatrix1,3), VScale(V4(LMatrix1,0), VDot(VSub(LP0, V4(LMatrix1,3)), V4(LMatrix1,0))));

NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP1[0], @LMatrix0[1, 0]);
NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP1[0], @LMatrix0[2, 0]);

// get a point along the ping axis at some reasonable large distance from the pivot
LQ0 := VAdd(LP0, VScale(V4(LMatrix0,0), MIN_JOINT_PIN_LENGTH));
LQ1 := VAdd(LP1, VScale(V4(LMatrix1,0), MIN_JOINT_PIN_LENGTH));

// two constraints row perpendicular to the pin
NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[0], @LMatrix0[1, 0]);
NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[0], @LMatrix0[2, 0]);

// get a point along the ping axis at some reasonable large distance from the pivot
LR0 := VAdd(LP0, VScale(V4(LMatrix0,1), MIN_JOINT_PIN_LENGTH));
LR1 := VAdd(LP0, VScale(V4(LMatrix1,1), MIN_JOINT_PIN_LENGTH));

// one constraint row perpendicular to the pin
NewtonUserJointAddLinearRow(FJoint, @LR0[0], @LR1[0], @LMatrix0[2, 0]);

if FLimitsOn then
 begin
 LDist := VDot(VSub(V4(LMatrix0,3), V4(LMatrix1,3)), V4(LMatrix0,0));
 if LDist < FMinDist then
  begin
  // get a point along the up vector and set a constraint
  NewtonUserJointAddLinearRow(FJoint, @LMatrix0[3, 0], @LMatrix0[3, 0], @LMatrix0[0, 0]);

  // allow the object to return but not to kick going forward
  NewtonUserJointSetRowMinimumFriction(FJoint, 0.0);
  end
 else
  if LDist > FMaxDist then
   begin
   // invert the matrix for the stop limits on the other side. this is a bug fix for the origional SDK joint
   Matrix_Inverse(LMatrix1);

   // get a point along the up vector and set a constraint
   NewtonUserJointAddLinearRow(FJoint, @LMatrix1[3, 0], @LMatrix1[3, 0], @LMatrix1[0, 0]);

   // allow the object to return but not to kick going forward
   NewtonUserJointSetRowMinimumFriction(FJoint, 0.0);
   end;
 end;
end;

// *****************************************************************************
// *****************************************************************************
//  Hinge
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointHinge.Create(aPivot, aPin: TVector3f; aChild, aParent: PNewtonBody);
Begin
inherited Create(6, aChild, aParent);
FLimitsOn     := False;
FMinAngle     := -45 * PI / 180;
FMaxAngle     := 45 * PI / 180;
// calculate the two local matrix of the pivot point
CalculateLocalMatrix(aPivot, aPin, FLocalMatrix0, FLocalMatrix1);
end;

procedure TNewtonCustomJointHinge.EnableLimits(aEnabled: Boolean);
begin
FLimitsOn := aEnabled;
end;

procedure TNewtonCustomJointHinge.SetLimits(aMinAngle, aMaxAngle: Single);
begin
FMinAngle := aMinAngle * PI / 180;
FMaxAngle := aMaxAngle * PI / 180;
end;

procedure TNewtonCustomJointHinge.SubmitConstraint;
var
 LAngle    : Single;
 LSinAngle : Single;
 LCosAngle : Single;
 LRelAngle : Single;
 LMatrix0  : TMatrix4f;
 LMatrix1  : TMatrix4f;
 LQ0       : TVector4f;
 LQ1       : TVector4f;
begin
if FJoint = nil then
 exit;

// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(FLocalMatrix0, FLocalMatrix1, LMatrix0, LMatrix1);

// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(FJoint, @LMatrix0[3, 0], @LMatrix1[3, 0], @LMatrix0[0, 0]);
NewtonUserJointAddLinearRow(FJoint, @LMatrix0[3, 0], @LMatrix1[3, 0], @LMatrix0[1, 0]);
NewtonUserJointAddLinearRow(FJoint, @LMatrix0[3, 0], @LMatrix1[3, 0], @LMatrix0[2, 0]);

// get a point along the pin axis at some reasonable large distance from the pivot
LQ0 := VAdd(V4(LMatrix0,3), VScale(V4(LMatrix0,0), MIN_JOINT_PIN_LENGTH));
LQ1 := VAdd(V4(LMatrix0,3), VScale(V4(LMatrix0,0), MIN_JOINT_PIN_LENGTH));

// two constraints row perpendiculars to the hinge pin
NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[0], @LMatrix0[1, 0]);
NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[0], @LMatrix0[2, 0]);

// the joint angle can be determine by getting the angle between any two non parallel vectors
LSinAngle := VDot(V4(LMatrix0,0), VCross(V4(LMatrix0,1), V4(LMatrix1,1)));
LCosAngle := VDot(V4(LMatrix0,1), V4(LMatrix1,1));
LAngle    := ArcTan2(LSinAngle, LCosAngle);

// Limit angular movement
if FLimitsOn then
 begin
 if LAngle < FMinAngle then
  begin
  LRelAngle := LAngle - FMinAngle;
  // tell joint error will minimize the exceeded angle error
  NewtonUserJointAddAngularRow(FJoint, LRelAngle, @LMatrix0[0, 0]);
  // need high stiffness here
  NewtonUserJointSetRowStiffness(FJoint, 1.0);
  end
 else
  if LAngle > FMaxAngle then
   begin
   LRelAngle := LAngle - FMaxAngle;
   // tell joint error will minimize the exceeded angle error
   NewtonUserJointAddAngularRow(FJoint, LRelAngle, @LMatrix0[0, 0]);
   // need high stiffness here
   NewtonUserJointSetRowStiffness(FJoint, 1.0);
   end;
 end;
end;

// *****************************************************************************
// *****************************************************************************
//  Dry Rolling Friction
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointDryRollingFriction.Create(aRadius, aCoefficient: Single; aChild: PNewtonBody);
var
 LMass : Single;
 LIxx  : Single;
 LIyy  : Single;
 LIzz  : Single;
Begin
inherited
Create(1, aChild, nil);
NewtonBodyGetMassMatrix(aChild, @LMass, @LIxx, @LIyy, @LIzz);
FFrictionCoef   := aCoefficient;
FFrictionTorque := LIxx * aRadius;
end;

// rolling friction works as follow: the idealization of the contact of a spherical object
// with a another surface is a point that pass by the center of the sphere.
// in most cases this is enough to model the collision but in insufficient for modeling
// the rolling friction. In reality contact with the sphere with the other surface is not
// a point but a contact patch. A contact patch has the property the it generates a fix
// constant rolling torque that opposes the movement of the sphere.
// we can model this torque by adding a clamped torque aligned to the instantaneously axis
// of rotation of the ball. and with a magnitude of the stopping angular acceleration.
procedure TNewtonCustomJointDryRollingFriction.SubmitConstraint;
var
 LOmega          : TVector3f;
 LPin            : TVector3f;
 LTime           : Single;
 LOmegaMag       : Single;
 LTorqueFriction : Single;
Begin
// get the omega vector
NewtonBodyGetOmega(FBody0, @LOmega[0]);

LOmegaMag := sqrt(VDot(LOmega, LOmega));

if LOmegaMag > 0.1 then
 begin
 // Tell Newton to use the friction of the omega vector to apply the rolling friction
 LPin := VScale(LOmega, 1.0 / LOmegaMag);
 NewtonUserJointAddAngularRow(FJoint, 0.0, @LPin[0]);

 // calculate the acceleration to stop the ball in one time step
 LTime := NewtonGetTimeStep(FWorld);
 NewtonUserJointSetRowAcceleration(FJoint, -LOmegaMag / LTime);

 // set the friction limit proportional the sphere Inertia
 LTorqueFriction := FFrictionTorque * FFrictionCoef;
 NewtonUserJointSetRowMinimumFriction(FJoint, -LTorqueFriction);
 NewtonUserJointSetRowMaximumFriction(FJoint, LTorqueFriction);
 end
else
 begin
 // when omega is too low scale a little bit and damp the omega directly
 VScale(LOmega, 0.2);
 NewtonBodySetOmega(FBody0, @LOmega[0]);
 end;
end;


// *****************************************************************************
// *****************************************************************************
//  Universal
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointUniversal.Create(aPivot, aPin0, aPin1: TVector3f; aChild, aParent: PNewtonBody);
var
 LMatrix0      : TMatrix4f;
 LMatrix1      : TMatrix4f;
 LPinAndPivot  : TMatrix4f;
 LMatrixInvert : TMatrix4f;
 LPin0         : TVector4f;
 LPin1         : TVector4f;
begin
inherited
Create(6, aChild, aParent);

LPin0 := V4(aPin0[0], aPin0[1], aPin0[2]);
LPin1 := V4(aPin1[0], aPin1[1], aPin1[2]);

FMinAngle0 := -45 * PI / 180;
FMaxAngle0 :=  45 * PI / 180;

FMinAngle1 := -45 * PI / 180;
FMaxAngle1 :=  45 * PI / 180;

FAngularDamp0  := 0.5;
FAngularAccel0 := -4;

FAngularDamp1  := 0.3;
FAngularAccel1 := -4;

// get the global matrices of each rigid body.
NewtonBodyGetMatrix(FBody0, @LMatrix0[0, 0]);
LMatrix1 := IdentityMatrix;
if FBody1 <> nil then
  NewtonBodyGetMatrix(FBody1, @LMatrix1[0, 0]);

// create a global matrix at the pivot point with front vector aligned to the pin vector
Matrix_SetColumn(LPinAndPivot, 0, VScale(LPin0, 1.0 / sqrt(VDot(LPin0, LPin0))));
Matrix_SetColumn(LPinAndPivot, 2, VCross(LPin0, LPin1));
Matrix_SetColumn(LPinAndPivot, 2, VScale(V4(LPinAndPivot,2), 1.0 / sqrt(VDot(V4(LPinAndPivot,2), V4(LPinAndPivot,2)))));
Matrix_SetColumn(LPinAndPivot, 1, VCross(V4(LPinAndPivot,2), V4(LPinAndPivot,0)));
Matrix_SetColumn(LPinAndPivot, 3, V4(aPivot[0], aPivot[1], aPivot[2], 1));

// calculate the relative matrix of the pin and pivot on each body
LMatrixInvert := LMatrix0;
Matrix_Inverse(LMatrixInvert);
FLocalMatrix0 := Matrix_Multiply(LPinAndPivot, LMatrixInvert);

LMatrixInvert := LMatrix1;
Matrix_Inverse(LMatrixInvert);
FLocalMatrix1 := Matrix_Multiply(LPinAndPivot, LMatrixInvert);
end;

procedure TNewtonCustomJointUniversal.EnableLimit0(aEnabled: Boolean);
begin
FLimit0on := aEnabled;
end;

procedure TNewtonCustomJointUniversal.EnableLimit1(aEnabled: Boolean);
begin
FLimit1on := aEnabled;
end;

procedure TNewtonCustomJointUniversal.SetLimits0(aMinAngle, aMaxAngle: Single);
begin
FMinAngle0 := aMinAngle * PI / 180;
FMaxAngle0 := aMaxAngle * PI / 180;
end;

procedure TNewtonCustomJointUniversal.SetLimits1(aMinAngle, aMaxAngle: Single);
begin
FMinAngle1 := aMinAngle * PI / 180;
FMaxAngle1 := aMaxAngle * PI / 180;
end;

procedure TNewtonCustomJointUniversal.EnableMotor0(aEnabled: Boolean);
begin
FAngularMotor0On := aEnabled;
end;

procedure TNewtonCustomJointUniversal.EnableMotor1(aEnabled: Boolean);
begin
FAngularMotor1On := aEnabled;
end;

procedure TNewtonCustomJointUniversal.SubmitConstraint;
var
 LAngle    : Single;
 LSinAngle : Single;
 LCosAngle : Single;
 LRelAngle : Single;
 LRelOmega : Single;
 LRelAccel : Single;
 LMatrix0  : TMatrix4f;
 LMatrix1  : TMatrix4f;
 LDir0     : TVector4f;
 LDir1     : TVector4f;
 LDir2     : TVector4f;