newtoncustomjoints.pas

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

 LDir3     : TVector4f;
 LP0       : TVector4f;
 LP1       : TVector4f;
 LQ0       : TVector4f;
 LQ1       : TVector4f;
 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);

// get the pin fixed to the first body
LDir0 := V4(LMatrix0, 0);
// get the pin fixed to the second body
LDir1 := V4(LMatrix1, 1);

// construct an othoganal coordinate system with these two vectors
LDir2 := VCross(LDir0, LDir1);
LDir3 := VCross(LDir2, LDir0);
LDir3 := VScale(LDir3, 1.0 / sqrt(VDot(LDir3, LDir3)));

LP0 := V4(LMatrix0, 3);
LP1 := V4(LMatrix1, 3);

LQ0 := VAdd(LP0, VScale(LDir3, MIN_JOINT_PIN_LENGTH));
LQ1 := VAdd(LP1, VScale(LDir1, MIN_JOINT_PIN_LENGTH));

NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP1[0], @LDir0[0]);
NewtonUserJointSetRowStiffness(FJoint, 1.0);

NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP1[0], @LDir1[0]);
NewtonUserJointSetRowStiffness(FJoint, 1.0);

NewtonUserJointAddLinearRow(FJoint, @LP0[0], @LP1[0], @LDir2[0]);
NewtonUserJointSetRowStiffness(FJoint, 1.0);

NewtonUserJointAddLinearRow(FJoint, @LQ0[0], @LQ1[0], @LDir0[0]);
NewtonUserJointSetRowStiffness(FJoint, 1.0);

if FLimit0On then
 begin
 LSinAngle := VDot(V4(LMatrix1,1), VCross(V4(LMatrix0,0), V4(LMatrix1,0)));
 LCosAngle := VDot(V4(LMatrix0,0), V4(LMatrix1,0));
 LAngle    := ArcTan2(LSinAngle, LCosAngle);
 if LAngle < FMinAngle0 then
  begin
  LRelAngle := LAngle - FMinAngle0;
  // tell joint error will minimize the exceeded angle error
  NewtonUserJointAddAngularRow(FJoint, LRelAngle, @LMatrix1[1, 0]);
  // need high stiffness here
  NewtonUserJointSetRowStiffness(FJoint, 1.0);
  end
 else
  if LAngle > FMaxAngle0 then
   begin
   LRelAngle := LAngle - FMaxAngle0;
   // tell joint error will minimize the exceeded angle error
   NewtonUserJointAddAngularRow(FJoint, LRelAngle, @LMatrix1[1, 0]);
   // need high stiffness here
   NewtonUserJointSetRowStiffness(FJoint, 1.0);
   end;
 end
 else
  if FAngularMotor0On then
   begin
   // get relative angular velocity
   NewtonBodyGetOmega(FBody0, @LOmega0[0]);
   if FBody1 <> nil then
    NewtonBodyGetOmega(FBody1, @LOmega1[0])
   else
    LOmega1 := V4(0,0,0,0);
   // calculate the desired acceleration
   LRelOmega := VDot(VSub(LOmega0, LOmega1), V4(LMatrix1, 1));
   LRelAccel := FAngularAccel0 - FAngularDamp0 * LRelOmega;
   // add an angular constraint row that will set the relative acceleration to zero
   NewtonUserJointAddAngularRow(FJoint, 0.0, @LMatrix1[1, 0]);
   NewtonUserJointSetRowAcceleration(FJoint, LRelAccel);
   end;

if FLimit1On then
 begin
 LSinAngle := VDot(V4(LMatrix0, 0), VCross(V4(LMatrix0, 1), V4(LMatrix1, 1)));
 LCosAngle := VDot(V4(LMatrix0, 1), V4(LMatrix1, 1));
 LAngle    := ArcTan2(LSinAngle, LCosAngle);
 if LAngle < FMinAngle1 then
  begin
  LRelAngle := LAngle - FMinAngle1;
  // 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 > FMaxAngle1 then
   begin
   LRelAngle := LAngle - FMaxAngle1;
   // 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
 else
  if FAngularMotor1On then
   begin
   // get relative angular velocity
   NewtonBodyGetOmega(FBody0, @LOmega0[0]);
   if FBody1 <> nil then
     NewtonBodyGetOmega(FBody1, @LOmega1[0])
   else
    LOmega1 := V4(0,0,0,0);
   // calculate the desired acceleration
   LRelOmega := VDot(VSub(LOmega0, LOmega1), V4(LMatrix0, 0));
   LRelAccel := FAngularAccel1 - FAngularDamp1 * LRelOmega;
   // add an angular constraint row that will set the relative acceleration to zero
   NewtonUserJointAddAngularRow(FJoint, 0.0, @LMatrix0[0, 0]);
   NewtonUserJointSetRowAcceleration(FJoint, LRelAccel);
   end;
end;

// *****************************************************************************
// *****************************************************************************
//  Gear
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointGear.Create(aGearRatio : Single; aChildPin, aParentPin : TVector3f; aParentBody, aChildBody : PNewtonBody);
var
 Pivot       : TVector3f;
 DummyMatrix : TMatrix4f;
begin
inherited Create(1, aChildBody, aParentBody);
FGearRatio := aGearRatio;
// calculate the two local matrix of the pivot point
Pivot := V3(0, 0, 0);
// calculate the local matrix for body body0
CalculateLocalMatrix(Pivot, aChildPin, FLocalMatrix0, DummyMatrix);
// calculate the local matrix for body body1
CalculateLocalMatrix(Pivot, aParentPin, DummyMatrix, FlocalMatrix1);
end;

procedure TNewtonCustomJointGear.SubmitConstraint;
var
 w0, w1    : Single;
 Time      : Single;
 RelAccel  : Single;
 RelOmega  : Single;
 Omega0    : TVector3f;
 Omega1    : TVector3f;
 Matrix0   : TMatrix4f;
 Matrix1   : TMatrix4f;
 Jacobian0 : array[0..5] of Single;
 Jacobian1 : array[0..5] of Single;
begin
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(FLocalMatrix0, FLocalMatrix1, Matrix0, Matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetOmega(FBody0, @Omega0[0]);
NewtonBodyGetOmega(FBody1, @Omega1[0]);
// get angular velocity relative to the pin vector
w0 := VDot(Omega0, V3(Matrix0, 0));
w1 := VDot(Omega1, V3(Matrix1, 0));
// establish the gear equation.
RelOmega := w0 + FGearRatio * w1;
// calculate the relative angular acceleration by dividing by the time step
Time     := NewtonGetTimeStep(FWorld);
RelAccel := -RelOmega / Time;
// set the linear part of Jacobian 0 to zero
Jacobian0[0] := 0;
Jacobian0[1] := 0;
Jacobian0[2] := 0;
// set the angular part of Jacobian 0 pin vector
Jacobian0[3] := V3(Matrix0, 0)[0];
Jacobian0[4] := V3(Matrix0, 0)[1];
Jacobian0[5] := V3(Matrix0, 0)[2];
// set the linear part of Jacobian 1 to zero
Jacobian1[0] :=	0;
Jacobian1[1] :=	0;
Jacobian1[2] :=	0;
// set the angular part of Jacobian 1 pin vector
Jacobian1[3] := V3(matrix1, 0)[0];
Jacobian1[4] := V3(matrix1, 0)[1];
Jacobian1[5] := V3(matrix1, 0)[2];
// add a angular constraint
NewtonUserJointAddGeneralRow(FJoint, @Jacobian0[0], @Jacobian1[0]);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration(FJoint, RelAccel);
end;


// *****************************************************************************
// *****************************************************************************
//  Pulley
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointPulley.Create(aGearRatio : Single; aChildPin, aParentPin : TVector3f; aParentBody, aChildBody : PNewtonBody);
var
 Pivot       : TVector3f;
 DummyMatrix : TMatrix4f;
begin
inherited Create(1, aChildBody, aParentBody);
FGearRatio := aGearRatio;
// calculate the two local matrix of the pivot point
Pivot := V3(0, 0, 0);
// calculate the local matrix for body body0
CalculateLocalMatrix(Pivot, aChildPin, FLocalMatrix0, DummyMatrix);
// calculate the local matrix for body body1
CalculateLocalMatrix(Pivot, aParentPin, DummyMatrix, FlocalMatrix1);
end;

procedure TNewtonCustomJointPulley.SubmitConstraint;
var
 w0, w1    : Single;
 Time      : Single;
 RelAccel  : Single;
 RelVeloc  : Single;
 Veloc0    : TVector3f;
 Veloc1    : TVector3f;
 Matrix0   : TMatrix4f;
 Matrix1   : TMatrix4f;
 Jacobian0 : array[0..5] of Single;
 Jacobian1 : array[0..5] of Single;
begin
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(FLocalMatrix0, FLocalMatrix1, Matrix0, Matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetVelocity(FBody0, @Veloc0[0]);
NewtonBodyGetVelocity(FBody1, @Veloc1[0]);
// get angular velocity relative to the pin vector
w0 := VDot(Veloc0, V3(Matrix0, 0));
w1 := VDot(Veloc1, V3(Matrix1, 0));
// establish the gear equation.
RelVeloc := w0 + FGearRatio * w1;
// calculate the relative angular acceleration by dividing by the time step
Time     := NewtonGetTimeStep(FWorld);
RelAccel := -RelVeloc / Time;
// set the linear part of Jacobian 0 to translational pin vector
Jacobian0[0] := V3(Matrix0, 0)[0];
Jacobian0[1] := V3(Matrix0, 0)[1];
Jacobian0[2] := V3(Matrix0, 0)[2];
// set the rotational part of Jacobian 0 to zero
Jacobian0[3] := 0;
Jacobian0[4] := 0;
Jacobian0[5] := 0;
// set the linear part of Jacobian 1 to translational pin vector
Jacobian1[0] :=	V3(matrix1, 0)[0];
Jacobian1[1] :=	V3(matrix1, 0)[1];
Jacobian1[2] :=	V3(matrix1, 0)[2];
// set the rotational part of Jacobian 1 to zero
Jacobian1[3] := 0;
Jacobian1[4] := 0;
Jacobian1[5] := 0;
// add a angular constraint
NewtonUserJointAddGeneralRow(FJoint, @Jacobian0[0], @Jacobian1[0]);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration(FJoint, RelAccel);
end;


// *****************************************************************************
// *****************************************************************************
//  Worm Gear
// *****************************************************************************
// *****************************************************************************
constructor TNewtonCustomJointWormGear.Create(aGearRatio : Single; aRotationalPin, aLinearPin : TVector3f; aRotationalBody, aLinearBody : PNewtonBody);
var
 Pivot       : TVector3f;
 DummyMatrix : TMatrix4f;
begin
inherited Create(1, aRotationalBody, aLinearBody);
FGearRatio := aGearRatio;
// calculate the two local matrix of the pivot point
Pivot := V3(0, 0, 0);
// calculate the local matrix for body body0
CalculateLocalMatrix(Pivot, aRotationalPin, FLocalMatrix0, DummyMatrix);
// calculate the local matrix for body body1
CalculateLocalMatrix(Pivot, aLinearPin, DummyMatrix, FlocalMatrix1);
end;

procedure TNewtonCustomJointWormGear.SubmitConstraint;
var
 w0, w1    : Single;
 Time      : Single;
 RelAccel  : Single;
 RelVeloc  : Single;
 Omega0    : TVector3f;
 Veloc1    : TVector3f;
 Matrix0   : TMatrix4f;
 Matrix1   : TMatrix4f;
 Jacobian0 : array[0..5] of Single;
 Jacobian1 : array[0..5] of Single;
begin
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(FLocalMatrix0, FLocalMatrix1, Matrix0, Matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetOmega(FBody0, @Omega0[0]);
NewtonBodyGetVelocity(FBody1, @Veloc1[0]);
// get angular velocity relative to the pin vector
w0 := VDot(Omega0, V3(Matrix0, 0));
w1 := VDot(Veloc1, V3(Matrix1, 0));
// establish the gear equation.
RelVeloc := w0 + FGearRatio * w1;
// calculate the relative angular acceleration by dividing by the time step
Time     := NewtonGetTimeStep(FWorld);
RelAccel := -RelVeloc / Time;
// set the linear part of Jacobian 0 to zero
Jacobian0[0] := 0;
Jacobian0[1] := 0;
Jacobian0[2] := 0;
// set the rotational part of Jacobian 0 to pin vector
Jacobian0[3] := V3(Matrix0, 0)[0];
Jacobian0[4] := V3(Matrix0, 0)[0];
Jacobian0[5] := V3(Matrix0, 0)[0];
// set the linear part of Jacobian 1 to translational pin vector
Jacobian1[0] :=	V3(matrix1, 0)[0];
Jacobian1[1] :=	V3(matrix1, 0)[1];
Jacobian1[2] :=	V3(matrix1, 0)[2];
// set the rotational part of Jacobian 1 to zero
Jacobian1[3] := 0;
Jacobian1[4] := 0;
Jacobian1[5] := 0;
// add a angular constraint
NewtonUserJointAddGeneralRow(FJoint, @Jacobian0[0], @Jacobian1[0]);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration(FJoint, RelAccel);
end;


end.