newtoncustomjoints.pas

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

end;

procedure TNewtonCustomBaseJoint.CalculateGlobalMatrix(const aLocalMatrix0, aLocalMatrix1: TMatrix4f; out aMatrix0, aMatrix1: TMatrix4f);
var
 LBody0Matrix: TMatrix4f;
 LBody1Matrix: TMatrix4f;
begin
// get the global matrices of each body
NewtonBodyGetMatrix(FBody0, @LBody0Matrix[0,0]);
LBody1Matrix := IdentityMatrix;
if FBody1 <> nil then
 NewtonBodyGetMatrix(FBody1, @LBody1Matrix[0,0]);
aMatrix0 := Matrix_Multiply(aLocalMatrix0, LBody0Matrix);
aMatrix1 := Matrix_Multiply(aLocalMatrix1, LBody1Matrix);
end;

procedure TNewtonCustomBaseJoint.CalculateLocalMatrix(const aPivot, aDir: TVector3f; out aLocalMatrix0, aLocalMatrix1: TMatrix4f);
var
 LMatrix0             : TMatrix4f;
 LMatrix1             : TMatrix4f;
 LPinAndPivoTMatrix4f : TMatrix4f;
 LInverseMatrix       : TMatrix4f;
begin
// get the global matrices of each 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
LPinAndPivotMatrix4f := MatrixGrammSchmidt(aDir);
Matrix_SetTransform(LPinAndPivotMatrix4f, aPivot);

// calculate the relative matrix of the pin and pivot on each body
LInverseMatrix := LMatrix0;
Matrix_Inverse(LInverseMatrix);
aLocalMatrix0 := Matrix_Multiply(LPinAndPivotMatrix4f, LInverseMatrix);

LInverseMatrix := LMatrix1;
Matrix_Inverse(LInverseMatrix);
aLocalMatrix1 := Matrix_Multiply(LPinAndPivotMatrix4f, LInverseMatrix);
end;


// *****************************************************************************
// *****************************************************************************
//  Up Vector
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointUpVector.Create(aPin: TVector3f; aBody: PNewtonBody);
var
 LPivot    : TMatrix4f;
 LPivotPos : TVector3f;
begin
inherited
Create(2, aBody, nil);
NewtonBodyGetMatrix(aBody, @LPivot[0, 0]);
LPivotPos := V3(LPivot[3, 0], LPivot[3, 1], LPivot[3, 2]);
CalculateLocalMatrix(LPivotPos, aPin, FLocalMatrix0, FLocalMatrix1);
end;

procedure TNewtonCustomJointUpVector.SetPinDir(const aDir: TVector3f);
Begin
FLocalMatrix1 := MatrixGrammSchmidt(aDir);
end;

procedure TNewtonCustomJointUpVector.SubmitConstraint;
var
 LMag        : Single;
 LAngle      : Single;
 LMatrix0    : TMatrix4f;
 LMatrix1    : TMatrix4f;
 LLateralDir : TVector4f;
 LFrontDir   : TVector4f;
begin
// calculate the position of the pivot point and the jacoviam direction vectors, in global space.
CalculateGlobalMatrix(FLocalMatrix0, FLocalMatrix1, LMatrix0, LMatrix1);

// if the body has rotated by some amount, there will be a plane of rotation
LLateralDir := VCross(V4(LMatrix0[0,0], LMatrix0[1,0], LMatrix0[2,0], LMatrix0[3,0]),
                      V4(LMatrix1[0,0], LMatrix1[1,0], LMatrix1[2,0], LMatrix1[3,0]));

LMag := VDot(LLateralDir, LLateralDir);
if LMag > 1.0e-6 then
 begin
 // if the vector is not 0 it means the body has rotated
 LMag := sqrt(LMag);
 VScale(LLateralDir, 1.0 / LMag);
 LAngle := ArcSin(LMag);

 // add an angular constraint to correct the error angle
 NewtonUserJointAddAngularRow(FJoint, LAngle, @LLateralDir[0]);

 // in theory only one correction is needed, but this produces instability as the body may move sideway.
 // a lateral correction prevent this from happening.
 LFrontDir := VCross(LLateralDir, V4(LMatrix1[0,0], LMatrix1[1,0], LMatrix1[2,0], LMatrix1[3,0]));
 NewtonUserJointAddAngularRow(FJoint, 0.0, @LFrontDir[0]);
 end
else
 begin
 // if the angle error is very small then two angular correction along the plane axis do the trick
 NewtonUserJointAddAngularRow(FJoint, 0.0, @LMatrix0[1, 0]);
 NewtonUserJointAddAngularRow(FJoint, 0.0, @LMatrix0[2, 0]);
 end;
end;


// *****************************************************************************
// *****************************************************************************
//  Ball and Socket
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointBallAndSocket.Create(aPivot: TVector3f; aChild, aParent: PNewtonBody);
var
 LMatrix : TMatrix4f;
 LPin    : TVector4f;
begin
inherited
Create(5, aChild, aParent);

// use as primary pin vector the line that goes from the pivot to the origin of body zero
// this eliminates some offset unwanted torques
NewtonBodyGetMatrix(FBody0, @LMatrix[0, 0]);

LPin := VSub(V4(LMatrix[0,3], LMatrix[1,3], LMatrix[2,3], LMatrix[3,3]), V4(aPivot[0], aPivot[1], aPivot[2], 0));

// the the pivot is already at the origin
if VDot(LPin, LPin) < 1.0e-3 then
 LPin := V4(LMatrix[0,0], LMatrix[1,0], LMatrix[2,0], LMatrix[3,0]);

// calculate the two local matrix of the pivot point
CalculateLocalMatrix(aPivot, V3(LPin[0], LPin[1], LPin[2]), FLocalMatrix0, FLocalMatrix1);
end;

procedure TNewtonCustomJointBallAndSocket.SubmitConstraint;
var
 LMatrix0 : TMatrix4f;
 LMatrix1 : TMatrix4f;
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
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]);
end;


// *****************************************************************************
// *****************************************************************************
//  Limited Ball And Socket
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointLimitedBallAndSocket.Create(aPivot, aConeDir: TVector3f; aConeAngle, aTwistAngle: Single; aChild, aParent: PNewtonBody);
Begin
inherited
Create(aPivot, aChild, aParent);
FConeAngle    := aConeAngle * PI / 180;
FTwistAngle   := aTwistAngle * PI / 180;
FCosConeAngle := Cos(FConeAngle);
// Recalculate local matrices so that the front vector align with the cone pin
CalculateLocalMatrix(aPivot, aConeDir, FLocalMatrix0, FLocalMatrix1);
end;

procedure TNewtonCustomJointLimitedBallAndSocket.SubmitConstraint;
var
 LConeCos        : Single;
 LMatrix0        : TMatrix4f;
 LMatrix1        : TMatrix4f;
 LP0             : TVector4f;
 LP1             : TVector4f;
 LLateralDir     : TVector4f;
 LUnitLateralDir : TVector4f;
 LTangentDir     : 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
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]);

// add a row to keep the child body inside the cone limit
// The child is inside the cone if the cos of the angle between the pin and
LConeCos := VDot(V4(LMatrix0[0,0], LMatrix0[1,0], LMatrix0[2,0], LMatrix0[3,0]), V4(LMatrix1[0,0], LMatrix1[1,0], LMatrix1[2,0], LMatrix1[3,0]));
if LConeCos < FCosConeAngle then
 begin
 // the child body has violated the cone limit we need to stop it from keep moving
 // for that we are going to pick a point along the the child body front vector
 LP0 := VAdd(V4(LMatrix0, 3), VScale(V4(LMatrix0,0), MIN_JOINT_PIN_LENGTH));
 LP1 := VAdd(V4(LMatrix1, 3), VScale(V4(LMatrix1,0), MIN_JOINT_PIN_LENGTH));

 // get a vectors perpendicular to the plane of motion
 LLateralDir := VCross(V4(LMatrix0,0), V4(LMatrix1,0));

 // note this could fail if the angle between matrix0.m_front and matrix1.m_front is 90 degree
 LUnitLateralDir := VScale(LLateralDir, 1.0 / sqrt(VDot(LLateralDir, LLateralDir)));

 // now we will add a constraint row along the lateral direction,
 // this will add stability as it will prevent the child body from moving sideways
 NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP0[0], @LUnitLateralDir[0]);

 // calculate the unit vector tangent to the trajectory
 LTangentDir := VCross(LUnitLateralDir, V4(LMatrix0,0));

 LP1 := VAdd(LP0, VScale(VSub(LP1, LP0), 0.2));
 NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP1[0], @LTangentdir[0]);

 // we need to allow the body to mo in opposite direction to the penetration
 // that can be done by setting the min friction to zero
 NewtonUserJointSetRowMinimumFriction(FJoint, 0.0);
 end;
end;


// *****************************************************************************
// *****************************************************************************
//  Corkscrew
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointCorkScrew.Create(aPivot, aPin: TVector3f; aChild, aParent: PNewtonBody);
Begin
inherited
Create(6, aChild, aParent);
FLimitsOn       := False;
FMinDist        := -1.0;
FMaxDist        := 1.0;
FAngularMotorOn := False;
FAngularDamp    := 0.1;
FAngularAccel   := 5.0;
// Calculate the two local matrix of the pivot point
CalculateLocalMatrix(aPivot, aPin, FLocalMatrix0, FLocalMatrix1);
end;

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

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

procedure TNewtonCustomJointCorkScrew.SubmitConstraint;
var
 LDist     : Single;
 LMatrix0  : TMatrix4f;
 LMatrix1  : TMatrix4f;
 LP0       : TVector4f;
 LP1       : TVector4f;
 LQ0       : TVector4f;
 LQ1       : TVector4f;
 LRelOmega : Single;
 LRelAccel : Single;
 LOmega0   : TVector4f;
 LOmega1   : 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], @LMatrix1[2, 0]);

// Get a point along the pin 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 perpendiculars to the hinge pin
NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[0], @LMatrix0[1, 0]);
NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[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);
  end
 else
  if LDist > FMaxDist 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
   NewtonUserJointSetRowMaximumFriction(FJoint, 0);
   end;
  end;

 if FAngularMotorOn then
  begin
  LOmega0 := V4(0, 0, 0);
  LOmega1 := V4(0, 0, 0);

  // get relative angular velocity
  NewtonBodyGetOmega(FBody0, @LOmega0[0]);

  if FBody1 <> nil then
   NewtonBodyGetOmega(FBody1, @LOmega1[0]);

  // calculate the desired acceleration
  LRelOmega := VDot(VSub(LOmega0, LOmega1), V4(LMatrix0,0));
  LRelAccel := FAngularAccel - FAngularDamp * LRelOmega;

  // if the motor capability is on, then set angular acceleration with zero angular correction
  NewtonUserJointAddAngularRow(FJoint, 0.0, @LMatrix0[0, 0]);

  // override the angular acceleration for this Jacobian to the desired acceleration
  NewtonUserJointSetRowAcceleration(FJoint, LRelAccel);
  end;
end;

// *****************************************************************************
// *****************************************************************************
//  Slider
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointSlider.Create(aPivot, aPin: TVector3f; aChild, aParent: PNewtonBody);
begin
inherited
Create(6, aChild, aParent);