oxnewtoncustomjoints.pas

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

  end;
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCHingeJoint last change by Dave Gravel.                   //
{******************************************************************************}
constructor TOXCHingeJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 6;
  LocalUse:= False;
  FJointLimitsOn:= false;
  FJointMinAngle:= -45.0;
  FJointMaxAngle:= 45.0;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCHingeJoint last change by Dave Gravel.                   //
{******************************************************************************}
procedure TOXCHingeJoint.CustomCreate( cBody, pBody: PNewtonBody; cPivot, cPin: TOXVector4 );
begin
  FBody0:= cBody;
  FBody1:= pBody;
  // Recalculate local matrices so that the front vector align with the cone pin
  CalculateLocalMatrix( oxV3Make( cPivot ),
                        oxV3Make( cPin ),
                        FcLocalMatrix0, FcLocalMatrix1 );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCHingeJoint last change by Dave Gravel.                   //
{******************************************************************************}
function TOXCHingeJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  q0: TOXVector3;
  q1: TOXVector3;
  angle: Float;
  relAngle: Float;
  sinAngle: Float;
  cosAngle: Float;
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  MinAngle: Float;
  MaxAngle: Float;
begin
  MinAngle:= FJointMinAngle * PI / 180.0;
  MaxAngle:= FJointMaxAngle * PI / 180.0;
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  CalculateGlobalMatrix( LocalMatrix0, LocalMatrix1, matrix0, matrix1 );
  // Restrict the movement on the pivot point along all tree orthonormal direction
  AddLinearRow( @Matrix0[3, 0], @Matrix1[3, 0], @Matrix0[0, 0] );
  AddLinearRow( @Matrix0[3, 0], @Matrix1[3, 0], @Matrix0[1, 0] );
  AddLinearRow( @Matrix0[3, 0], @Matrix1[3, 0], @Matrix0[2, 0] );
  // get a point along the pin axis at some reasonable large distance from the pivot
  q0:= oxVAdd( oxV3Make(Matrix0[3]), oxVScale( oxV3Make(Matrix0[0]), JointMinPinLength ) );
  q1:= oxVAdd( oxV3Make(Matrix0[3]), oxVScale( oxV3Make(Matrix0[0]), JointMinPinLength ) );
  // two constraints row perpendicular to the pin vector
  AddLinearRow( @q0[0], @q1[0], @matrix0[1, 0] );
  AddLinearRow( @q0[0], @q1[0], @matrix0[2, 0] );
  // if limit are enable ...
  if FJointLimitsOn then begin
    // the joint angle can be determine by getting the angle between any two non parallel vectors
    sinAngle:= oxVDotProduct( Matrix0[0], oxVCrossProduct( Matrix0[1], Matrix1[1] ) );
    cosAngle:= oxVDotProduct( Matrix0[1], Matrix1[1] );
    angle:= ArcTan2( sinAngle, cosAngle );
    if ( angle < MinAngle ) then begin
      relAngle:= angle - MinAngle;
      // tell joint error will minimize the exceeded angle error
      AddAngularRow( relAngle, @matrix0[0] );
      // need high stiffness here
      SetRowStiffness( 1.0 );
    end else
    if ( angle > MaxAngle ) then begin
      relAngle:= angle - MaxAngle;
      // tell joint error will minimize the exceeded angle error
      AddAngularRow( relAngle, @matrix0[0] );
      // need high stiffness here
      SetRowStiffness( 1.0 );
    end;
  end;
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUniversalJoint last change by Dave Gravel.               //
{******************************************************************************}
constructor TOXCUniversalJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 6;
  LocalUse:= False;
  FJointLimitOn0:= false;
  FJointLimitOn1:= false;
  FJointAngularmotorOn0:= false;
  FJointAngularmotorOn1:= false;
  FJointMinAngle0:= -45.0;
  FJointMaxAngle0:= 45.0;
  FJointMinAngle1:= -45.0;
  FJointMaxAngle1:= 45.0;
  FJointAngularDamp0:= 0.5;
  FJointAngularAccel0:= -4.0;
  FJointAngularDamp1:= 0.3;
  FJointAngularAccel1:= -4.0;
  OnCustomBothCreate:= CustomBothCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUniversalJoint last change by Dave Gravel.               //
{******************************************************************************}
procedure TOXCUniversalJoint.CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
var
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  matrixInvert: TOXMatrix;
  pinAndPivotMatrix: TOXMatrix;
begin
  FBody0:= cBody; FBody1:= pBody;
  // Get the global matrices of each rigid body.
  NewtonBodyGetMatrix( cBody, @matrix0[0, 0] );
  matrix1:= oxIdentityM;
  if pBody <> nil then
    NewtonBodyGetMatrix( pBody, @Matrix1[0, 0] );
  // create a global matrix at the pivot point with front vector aligned to the pin vector
  oxMSetColumn( pinAndPivotMatrix, 0,
  oxVScale( cPin, 1.0 / sqrt( oxVDotProduct( cPin, cPin ) ) ) );
  oxMSetColumn( pinAndPivotMatrix, 2,
  oxVCrossProduct( cPin, pPin ) );
  oxMSetColumn( pinAndPivotMatrix, 2,
  oxVScale( pinAndPivotMatrix[2], 1.0 / sqrt( oxVDotProduct( pinAndPivotMatrix[2], pinAndPivotMatrix[2] ) ) ) );
  oxMSetColumn( pinAndPivotMatrix, 1,
  oxVCrossProduct( pinAndPivotMatrix[2], pinAndPivotMatrix[0] ) );
  oxMSetColumn( pinAndPivotMatrix, 3,
  oxV4Make( cPivot[0], cPivot[1], cPivot[2], 1 ) ); 
  // calculate the relative matrix of the pin and pivot on each body
  MatrixInvert:= Matrix0;
  oxInvertM( MatrixInvert );
  FcLocalMatrix0:= oxMMultiply( pinAndPivotMatrix, MatrixInvert );
  MatrixInvert:= Matrix1;
  oxInvertM( MatrixInvert );
  FcLocalMatrix1:= oxMMultiply( pinAndPivotMatrix, MatrixInvert );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUniversalJoint last change by Dave Gravel.               //
{******************************************************************************}
function TOXCUniversalJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  p0: TOXVector3;
  p1: TOXVector3;
  q0: TOXVector3;
  q1: TOXVector3;
  dir0: TOXVector3;
  dir1: TOXVector3;
  dir2: TOXVector3;
  dir3: TOXVector3;
  angle: Float;
  omega0: TOXVector3;
  omega1: TOXVector3;
  relOmega: Float;
  relAccel: Float;
  relAngle: Float;
  sinAngle: Float;
  cosAngle: Float;
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  MinAngle0: Float;
  MaxAngle0: Float;
  MinAngle1: Float;
  MaxAngle1: Float;
begin
  MinAngle0:= FJointMinAngle0 * PI / 180.0;
  MaxAngle0:= FJointMaxAngle0 * PI / 180.0;
  MinAngle1:= FJointMinAngle1 * PI / 180.0;
  MaxAngle1:= FJointMaxAngle1 * PI / 180.0;
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  CalculateGlobalMatrix( LocalMatrix0, LocalMatrix1, matrix0, matrix1 );
  // get the pin fixed to the first body
  dir0:= oxV3Make(Matrix0[0]);
  // get the pin fixed to the second body
  dir1:= oxV3Make(Matrix1[1]);
  // construct an orthogonal coordinate system with these two vectors
  dir2:= oxVCrossProduct( dir0, dir1 );
  dir3:= oxVCrossProduct( dir2, dir0 );
  dir3:= oxVScale( dir3, 1.0 / sqrt( oxVDotProduct( dir3, dir3) ) );
  p0:= oxV3Make(Matrix0[3]);
  p1:= oxV3Make(Matrix1[3]);
  q0:= oxVAdd( p0, oxVScale( dir3, JointMinPinLength ) );
  q1:= oxVAdd( p1, oxVScale( dir1, JointMinPinLength ) );
  AddLinearRow( @p0[0], @p1[0], @dir0[0] );
  SetRowStiffness( 1.0 );
  AddLinearRow( @p0[0], @p1[0], @dir1[0] );
  SetRowStiffness( 1.0 );
  AddLinearRow( @p0[0], @p1[0], @dir2[0] );
  SetRowStiffness( 1.0 );
  AddLinearRow( @q0[0], @q1[0], @dir0[0] );
  SetRowStiffness( 1.0 );
  // if limit are enable ...
  if ( FJointLimitOn0 ) then begin
   sinAngle:= oxVDotProduct( oxVCrossProduct( Matrix0[0], Matrix1[0] ), Matrix1[1] );
   cosAngle:= oxVDotProduct( Matrix0[0], Matrix1[0] );
   angle:= ArcTan2( sinAngle, cosAngle );
   if ( angle < MinAngle0 ) then begin
     relAngle:= angle - MinAngle0;
     // tell joint error will minimize the exceeded angle error
     AddAngularRow( relAngle, @matrix1[1] );
     // need high stiffeners here
     SetRowStiffness( 1.0 );
   end else
   if ( angle > MaxAngle0 ) then begin
     relAngle:= angle - MaxAngle0;
     // tell joint error will minimize the exceeded angle error
     AddAngularRow( relAngle, @matrix1[1] );
     // need high stiffness here
     SetRowStiffness( 1.0 );
   end;
   end else
   if ( FJointAngularmotorOn0 ) then begin
     omega0:= oxV3Make( 0.0, 0.0, 0.0 );
     omega1:= oxV3Make( 0.0, 0.0, 0.0 );
     // get relative angular velocity
     if FBody0 <> nil then
       NewtonBodyGetOmega( Fbody0, @omega0[0] );
     if FBody1 <> nil then
       NewtonBodyGetOmega( Fbody1, @omega1[0] );
     // calculate the desired acceleration
     relOmega:= oxVDotProduct( oxVSubtract( omega0, omega1 ), oxV3Make(Matrix1[1]) );
     relAccel:= FJointAngularAccel0 - FJointAngularDamp0 * relOmega;
     // add and angular constraint row to that will set the relative acceleration to zero
     AddAngularRow( 0.0, @matrix1[1] );
     // override the joint acceleration.
     SetRowAcceleration( relAccel );
   end;
   // check is the joint limit are enable
   if ( FJointLimitOn1 ) then begin
     sinAngle:= oxVDotProduct( oxVCrossProduct( Matrix0[1], Matrix1[1] ), Matrix0[0] );
     cosAngle:= oxVDotProduct( Matrix0[1], Matrix1[1] );
     angle:= ArcTan2( sinAngle, cosAngle );
     if ( angle < MinAngle1 ) then begin
       relAngle:= angle - MinAngle1;
       // tell joint error will minimize the exceeded angle error
       AddAngularRow( relAngle, @matrix0[0] );
       // need high stiffeners here
       SetRowStiffness( 1.0 );
     end else
     if ( angle > MaxAngle1 ) then begin
       relAngle:= angle - MaxAngle1;
       // tell joint error will minimize the exceeded angle error
       AddAngularRow( relAngle, @matrix0[0] );
       // need high stiffness here
       SetRowStiffness( 1.0 );
     end;
     // check is the joint limit motor is enable
     end else
     if ( FJointAngularmotorOn1 ) then begin
       omega0:= oxV3Make( 0.0, 0.0, 0.0 );
       omega1:= oxV3Make( 0.0, 0.0, 0.0 );
       // get relative angular velocity
       if FBody0 <> nil then
         NewtonBodyGetOmega( Fbody0, @omega0[0] );
       if FBody1 <> nil then
         NewtonBodyGetOmega( Fbody1, @omega1[0] );
       // calculate the desired acceleration
       relOmega:= oxVDotProduct( oxVSubtract( omega0, omega1 ), oxV3Make(Matrix0[0]) );
       relAccel:= FJointAngularAccel1 - FJointAngularDamp1 * relOmega;
       // add and angular constraint row to that will set the relative acceleration to zero
       AddAngularRow( 0.0, @matrix0[0] );
       // override the joint acceleration.
       SetRowAcceleration( relAccel );
  end;
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCPulleyJoint last change by Dave Gravel.                  //
{******************************************************************************}
constructor TOXCPulleyJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 1;
  LocalUse:= False;
  FJointGearRatio:= 2.0;
  OnCustomBothCreate:= CustomBothCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCPulleyJoint last change by Dave Gravel.                  //
{******************************************************************************}
procedure TOXCPulleyJoint.CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
var