oxnewtoncustomjoints.pas

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

  AddLinearRow( @q0[0], @q1[0], @matrix0[1] );
  AddLinearRow( @q0[0], @q1[0], @matrix0[2] );
  // get a point along the ping axis at some reasonable large distance from the pivot
  r0:= oxVAdd( p0, oxVScale( matrix0[1], JointMinPinLength ) );
  r1:= oxVAdd( p1, oxVScale( matrix1[1], JointMinPinLength ) );
  // one constraint row perpendicular to the pin
  AddLinearRow( @r0[0], @r1[0], @matrix0[2] );
  // if limit are enable ...
  if ( FJointLimitsOn ) then begin
    dist:= oxVDotProduct( oxVSubtract( matrix0[3], matrix1[3] ), matrix0[0] );
    if ( dist < FJointMinDist ) then begin
      // get a point along the up vector and set a constraint
      AddLinearRow( @matrix0[3], @matrix0[3], @matrix0[0] );
      // allow the object to return but not to kick going forward
      SetRowMinimumFriction( 0.0 );
    end else
    if ( dist > FJointMaxDist ) then begin
      // get a point along the up vector and set a constraint
      AddLinearRow( @matrix0[3], @matrix0[3], @matrix0[0] );
      // allow the object to return but not to kick going forward
      SetRowMaximumFriction( 0.0 );
    end;
  end;
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCGearJoint last change by Dave Gravel.                    //
{******************************************************************************}
constructor TOXCGearJoint.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]: TOXCGearJoint last change by Dave Gravel.                    //
{******************************************************************************}
procedure TOXCGearJoint.CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
var
  Pivot: TOXVector3;
  DummyMatrix: TOXMatrix;
begin
  FBody0:= cBody;
  FBody1:= pBody;
  // calculate the two local matrix of the pivot point
  AffineVectorMake( 0, 0, 0 );
  // calculate the local matrix for body body0
  CalculateLocalMatrix( Pivot, oxV3Make( cPin ), FcLocalMatrix0, DummyMatrix );
  // calculate the local matrix for body body1
  CalculateLocalMatrix( Pivot, oxV3Make( pPin ), DummyMatrix, FcLocalMatrix1 );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCGearJoint last change by Dave Gravel.                    //
{******************************************************************************}
function TOXCGearJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  w0, w1: Float;
  Time: Float;
  relAccel: Float;
  relOmega: Float;
  omega0: TOXVector4;
  omega1: TOXVector4;
  Matrix0: TOXMatrix;
  Matrix1: TOXMatrix;
  Jacobian0: array[0..5] of Float;
  Jacobian1: array[0..5] of Float;
begin
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  CalculateGlobalMatrix( LocalMatrix0, LocalMatrix1, 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:= oxVDotProduct( omega0, matrix0[0] );
  w1:= oxVDotProduct( omega1, matrix1[0] );
  // establish the gear equation.
  relOmega:= w0 + FJointGearRatio * w1;
  // calculate the relative angular acceleration by dividing by the time step
  time:= NewtonGetTimeStep( Manager.World );
  relAccel:= - relOmega / time;
  // set the linear part of Jacobian 0 to zero
  jacobian0[0]:= 0.0;
  jacobian0[1]:= 0.0;
  jacobian0[2]:= 0.0;
  // set the angular part of Jacobian 0 pin vector
  jacobian0[3]:= matrix0[0][0];
  jacobian0[4]:= matrix0[0][1];
  jacobian0[5]:= matrix0[0][2];
  // set the linear part of Jacobian 1 to zero
  jacobian1[0]:= 0.0;
  jacobian1[1]:= 0.0;
  jacobian1[2]:= 0.0;
  // set the angular part of Jacobian 1 pin vector
  jacobian1[3]:= matrix1[0][0];
  jacobian1[4]:= matrix1[0][1];
  jacobian1[5]:= matrix1[0][2];
  // add a angular constraint
  AddGeneralRow( @jacobian0[0], @jacobian1[0] );
  // set the desired angular acceleration between the two bodies
  SetRowAcceleration( relAccel );
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCWormGearJoint last change by Dave Gravel.                //
{******************************************************************************}
constructor TOXCWormGearJoint.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]: TOXCWormGearJoint last change by Dave Gravel.                //
{******************************************************************************}
procedure TOXCWormGearJoint.CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
var
  Pivot: TOXVector3;
  DummyMatrix: TOXMatrix;
begin
  FBody0:= cBody;
  FBody1:= pBody;
  // calculate the two local matrix of the pivot point
  AffineVectorMake( 0, 0, 0 );
  // calculate the local matrix for body body0
  CalculateLocalMatrix( Pivot, oxV3Make( cPin ), FcLocalMatrix0, DummyMatrix );
  // calculate the local matrix for body body1
  CalculateLocalMatrix( Pivot, oxV3Make( pPin ), DummyMatrix, FcLocalMatrix1 );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCWormGearJoint last change by Dave Gravel.                //
{******************************************************************************}
function TOXCWormGearJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  w0: Float;
  w1: Float;
  time: Float;
  relAccel: Float;
  relVeloc: Float;
  omega0: TOXVector4;
  veloc1: TOXVector4;
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  Jacobian0: array[0..5] of Float;
  Jacobian1: array[0..5] of Float;
begin
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  CalculateGlobalMatrix( LocalMatrix0, LocalMatrix1, 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:= oxVDotProduct( omega0, matrix0[0] );
  w1:= oxVDotProduct( veloc1, matrix1[0] );
  // establish the gear equation.
  relVeloc:= w0 + FJointGearRatio * w1;
  // calculate the relative angular acceleration by dividing by the time step
  time:= NewtonGetTimeStep( Manager.World );
  relAccel:= - relVeloc / time;
  // set the linear part of Jacobian 0 to zero
  jacobian0[0]:= 0.0;
  jacobian0[1]:= 0.0;
  jacobian0[2]:= 0.0;
  // set the angular part of Jacobian 0 pin vector
  jacobian0[3]:= matrix0[0][0];
  jacobian0[4]:= matrix0[0][1];
  jacobian0[5]:= matrix0[0][2];
  // set the linear part of Jacobian 1 to translational pin vector
  jacobian1[0]:= matrix1[0][0];
  jacobian1[1]:= matrix1[0][1];
  jacobian1[2]:= matrix1[0][2];
  // set the rotational part of Jacobian 1 to zero
  jacobian1[3]:= 0.0;
  jacobian1[4]:= 0.0;
  jacobian1[5]:= 0.0;
  // add a angular constraint
  AddGeneralRow( @jacobian0[0], @jacobian1[0] );
  // set the desired angular acceleration between the two bodies
  SetRowAcceleration( relAccel );
  result:= 0;
end;
// This part is from my contribution.
{******************************************************************************}
// [15-9-2007]: TOXCUniversalControlJoint last change by Dave Gravel.        //
{******************************************************************************}
constructor TOXCUniversalControlJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 40.0;
  FBendX:= 0.0;
  FBendY:= 0.0;
  MaxDOF:= 6;
  LocalUse:= False;
  OnCustomBothCreate:= CustomBothCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUniversalControlJoint last change by Dave Gravel.        //
{******************************************************************************}
procedure TOXCUniversalControlJoint.CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
var
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  matrixInvert: TOXMatrix;
  pinAndPivotMatrix: TOXMatrix;
begin
  FBody0:= cBody; FBody1:= pBody;
  NewtonBodyGetMatrix( cBody, @matrix0[0, 0] );
  matrix1:= oxIdentityM;
  if pBody <> nil then NewtonBodyGetMatrix( pBody, @Matrix1[0, 0] );
  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 ) );
  MatrixInvert:= Matrix0;
  oxInvertM( MatrixInvert );
  FcLocalMatrix0:= oxMMultiply( pinAndPivotMatrix, MatrixInvert );
  MatrixInvert:= Matrix1;
  oxInvertM( MatrixInvert );
  FcLocalMatrix1:= oxMMultiply( pinAndPivotMatrix, MatrixInvert );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUniversalControlJoint last change by Dave Gravel.        //
{******************************************************************************}
function TOXCUniversalControlJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  p0: TOXVector3;
  p1: TOXVector3;
  q0: TOXVector3;
  q1: TOXVector3;
  dir0: TOXVector3;
  dir1: TOXVector3;
  dir2: TOXVector3;
  dir3: TOXVector3;
  angle: Float;
  relAngle: Float;
  sinAngle: Float;
  cosAngle: Float;
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
begin
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  CalculateGlobalMatrix( LocalMatrix0, LocalMatrix1, matrix0, matrix1 );
  dir0:= oxV3Make(Matrix0[0]);
  dir1:= oxV3Make(Matrix1[1]);
  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 );
  sinAngle:= oxVDotProduct( oxVCrossProduct( Matrix0[0], Matrix1[0] ), Matrix1[1] );
  cosAngle:= oxVDotProduct( Matrix0[0], Matrix1[0] );
  angle:= ArcTan2( sinAngle, cosAngle );
  if (FBendX>3.14) then FBendX:= 3.13;
  if (FBendX<-3.14) then FBendX:= -3.13;
  if (FBendY>3.14) then FBendY:= 3.13;
  if (FBendY<-3.14) then FBendY:= -3.13;
  if ( angle < FBendX ) then begin
    relAngle:= angle - FBendX;
    AddAngularRow( relAngle, @matrix1[1] );
    SetRowStiffness( 1.0 );
  end;
  if ( angle > FBendX ) then begin
    relAngle:= angle - FBendX;
    AddAngularRow( relAngle, @matrix1[1] );
    SetRowStiffness( 1.0 );
  end;
  sinAngle:= oxVDotPr