oxnewtoncustomjoints.pas

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

  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]: TOXCPulleyJoint last change by Dave Gravel.                  //
{******************************************************************************}
function TOXCPulleyJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  w0, w1: Float;
  Time: Float;
  RelAccel: Float;
  RelVeloc: Float;
  Veloc0: 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
  if FBody0<>nil then
    NewtonBodyGetVelocity( FBody0, @Veloc0[0] );
  if FBody1<>nil then
    NewtonBodyGetVelocity( FBody1, @Veloc1[0] );
  // get angular velocity relative to the pin vector
  w0:= oxVDotProduct( Veloc0, 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 translational pin vector
  Jacobian0[0]:= Matrix0[0][0];
  Jacobian0[1]:= Matrix0[0][1];
  Jacobian0[2]:= 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]:= 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;
  Jacobian1[4]:= 0;
  Jacobian1[5]:= 0;
  // 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]: TOXCDryRollingFrictionJoint last change by Dave Gravel.      //
{******************************************************************************}
constructor TOXCDryRollingFrictionJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  FPin:= oxV3Make( 0, 0, 0 );
  MaxDOF:= 1;
  LocalUse:= False;
  FFrictionCoef:= 0.0;
  FFrictionTorque:= 0.0;
  FJointRadius:= 1.0;
  FJointCoeficient:= 0.25;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCDryRollingFrictionJoint last change by Dave Gravel.      //
{******************************************************************************}
destructor TOXCDryRollingFrictionJoint.Destroy;
begin
  FBody0:= nil;
  FBody1:= nil;
  OnCustomCreate:= nil;
  OnCallBack:= nil;
  inherited destroy;
end;
{******************************************************************************}
// [15-9-2007]: TOXCDryRollingFrictionJoint last change by Dave Gravel.      //
{******************************************************************************}
procedure TOXCDryRollingFrictionJoint.CustomCreate( cBody, pBody: PNewtonBody; cPivot, cPin: TOXVector4 );
var
  mass: Float;
  Ixx: Float;
  Iyy: Float;
  Izz: Float;
begin
  FBody0:= cBody;
  FBody1:= pBody;
  NewtonBodyGetMassMatrix( cBody, @mass, @Ixx, @Iyy, @Izz );
  FFrictionCoef:= FJointCoeficient;
  FFrictionTorque:= Ixx * FJointRadius;
end;
{******************************************************************************}
// [15-9-2007]: TOXCDryRollingFrictionJoint last change by Dave Gravel.      //
{******************************************************************************}
function TOXCDryRollingFrictionJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  omega: TOXVector4;
  time: Float;
  omegaMag: Float;
  torqueFriction: Float;
begin
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  // 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.
  // get the omega vector
  // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
  if FBody0<>nil then
    NewtonBodyGetOmega( Fbody0, @omega[0] );
  omegaMag:= sqrt( oxVDotProduct( omega, omega ) );
  if ( omegaMag > 0.1 ) then begin
    FPin:= oxV3Make( oxVScale( omega, ( 1.0 / omegaMag ) ) );
    // tell newton to used this the friction of the omega vector to apply the rolling friction
    // Fix to make work the joint because when the vector come to 0,0,0 it create a error.
    // I'm not sure if the problem is 100% fixed with this trick,
    // I try to find the exact problem to find the best fix. 
    if ( FPin[0] <> 0 ) and ( FPin[1] <> 0 ) and ( FPin[2] <> 0 ) then begin
      AddAngularRow( 0.0, @FPin[0] );
    end else begin
      result:= 0;
      exit;
    end;
    // calculate the acceleration to stop the ball in one time step
    time:= NewtonGetTimeStep( Manager.World );
    SetRowAcceleration( -omegaMag / time );
    // set the friction limit proportional the sphere Inertia
    torqueFriction:= FFrictionTorque * FFrictionCoef;
    SetRowMinimumFriction( -torqueFriction );
    SetRowMaximumFriction( torqueFriction );
  end else begin
    // when omega is too low sheath a little bit and damp the omega directly
    omega:= oxVScale( omega, 0.2 );
    if FBody0<>nil then
      NewtonBodySetOmega( FBody0, @omega[0] );
  end;
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUpVectorJoint last change by Dave Gravel.                //
{******************************************************************************}
constructor TOXCUpVectorJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 2;
  LocalUse:= False;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUpVectorJoint last change by Dave Gravel.                //
{******************************************************************************}
procedure TOXCUpVectorJoint.UpdatePinDir;
var cVec: TOXVector3;
begin
  // bu animating the orientation of the pin vector the application can change the orientation of the picked object
  cVec[0]:= JointChildPin.X;
  cVec[1]:= JointChildPin.Y;
  cVec[2]:= JointChildPin.Z;
  LocalMatrix1:= oxMGrammSchmidt( cVec );
end;
{******************************************************************************}
// [15-9-2007]: TOXCUpVectorJoint last change by Dave Gravel.                //
{******************************************************************************}
procedure TOXCUpVectorJoint.CustomCreate( cBody, pBody: PNewtonBody; cPivot, cPin: TOXVector4 );
var
  pivot: TOXMatrix;
begin
  FBody0:= cBody;
  FBody1:= pBody;
  NewtonBodyGetMatrix( cBody, @pivot[0] );
  // Recalculate local matrices so that the front vector align with the cone pin
  CalculateLocalMatrix( oxV3Make( pivot[3] ),
                        oxV3Make( cPin ),
                        FcLocalMatrix0, FcLocalMatrix1 );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCUpVectorJoint last change by Dave Gravel.                //
{******************************************************************************}
function TOXCUpVectorJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  mag: Float;
  angle: Float;
  frontDir: TOXVector4;
  lateralDir: TOXVector4;
  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 );
  // if the body ha rotated by some amount, the there will be a plane of rotation
  lateralDir:= oxVCrossProduct( matrix0[0], matrix1[0] );
  mag:= oxVDotProduct( lateralDir, lateralDir );
  if ( mag > 1.0e-6 ) then begin
    // if the side vector is not zero, it means the body has rotated
    mag:= sqrt( mag );
    lateralDir:= oxVScale( lateralDir, 1.0 / mag );
    angle:= sin( mag );
    // add an angular constraint to correct the error angle
    AddAngularRow( angle, @lateralDir[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.
    frontDir:= oxVCrossProduct( lateralDir, matrix1[0] );
    AddAngularRow( 0.0, @frontDir[0] );
  end else begin
    // if the angle error is very small then two angular correction along the plane axis do the trick
    AddAngularRow( 0.0, @matrix0[1] );
    AddAngularRow( 0.0, @matrix0[2] );
  end;
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCSliderJoint last change by Dave Gravel.                  //
{******************************************************************************}
constructor TOXCSliderJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 6;
  LocalUse:= False;
  FJointLimitsOn:= false;
  FJointMinDist:= -1.0;
  FJointMaxDist:=  1.0;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCSliderJoint last change by Dave Gravel.                  //
{******************************************************************************}
procedure TOXCSliderJoint.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]: TOXCSliderJoint last change by Dave Gravel.                  //
{******************************************************************************}
function TOXCSliderJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  dist: Float;
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  p0: TOXVector4;
  p1: TOXVector4;
  q0: TOXVector4;
  q1: TOXVector4;
  r0: TOXVector4;
  r1: TOXVector4;
begin
  // 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
  p0:= matrix0[3];
  p1:= oxVAdd( Matrix1[3], oxVScale( Matrix1[0], oxVDotProduct( oxVSubtract( p0, Matrix1[3] ), Matrix1[0] ) ) );
  AddLinearRow( @p0[0], @p1[0], @matrix0[1] );
  AddLinearRow( @p0[0], @p1[0], @matrix0[2] );
  // get a point along the ping axis at some reasonable large distance from the pivot
  q0:= oxVAdd( p0, oxVScale( matrix0[0], JointMinPinLength ) );
  q1:= oxVAdd( p1, oxVScale( matrix1[0], JointMinPinLength ) );
  // two constraints row perpendicular to the pin