oxnewtoncustomjoints.pas

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

      FOrionX3D: byte;
      FcLocalMatrix0: TOXMatrix;
      FcLocalMatrix1: TOXMatrix;
      FJointGearRatio: Float;
      FBody0, FBody1: PNewtonBody;
      procedure CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
    public
      function SubmitConstrainst( userJoint: PNewtonJoint ): cardinal;
      constructor Create( aOwner: TComponent ); override;
    published
      property JointGearRatio: Float read FJointGearRatio write FJointGearRatio;
  end;
{******************************************************************************}
// [15-9-2007]: TOXCUniversalControlJoint last change by Dave Gravel.        //
{******************************************************************************}
type
  TOXCUniversalControlJoint = class( TOXUserBilateralJoint )
    private
      FOrionX3D: byte;
      FBendX: Float;
      FBendY: Float;
      FcLocalMatrix0: TOXMatrix;
      FcLocalMatrix1: TOXMatrix;
      FBody0, FBody1: PNewtonBody;
      procedure CustomBothCreate( cBody, pBody: PNewtonBody; cPivot, cPin, pPivot, pPin: TOXVector4 );
    public
      function SubmitConstrainst( userJoint: PNewtonJoint ): cardinal;
      constructor Create( aOwner: TComponent ); override;
    published
      property BendX: Float read FBendX write FBendX;
      property BendY: Float read FBendY write FBendY;
  end;
{******************************************************************************}
// [15-9-2007]: TOXCShockJoint last change by Dave Gravel.                   //
{******************************************************************************}
type
  TOXCShockJoint = class( TOXUserBilateralJoint )
    private
      FOrionX3D: byte;
      FcLocalMatrix0: TOXMatrix;
      FcLocalMatrix1: TOXMatrix;
      FBody0, FBody1: PNewtonBody;
      FlinearK: Float;
      FlinearD1: Float;
      FlinearD2: Float;
      FAngularK: Float;
      FAngularD1: Float;
      FAngularD2: Float;
      procedure CustomCreate( cBody, pBody: PNewtonBody; cPivot, cPin: TOXVector4 );
    public
      function SubmitConstrainst( userJoint: PNewtonJoint ): cardinal;
      constructor Create( aOwner: TComponent ); override;
    published
      property linearK: Float read FlinearK write FlinearK;
      property linearD1: Float read FlinearD1 write FlinearD1;
      property linearD2: Float read FlinearD2 write FlinearD2;
      property AngularK: Float read FAngularK write FAngularK;
      property AngularD1: Float read FAngularD1 write FAngularD1;
      property AngularD2: Float read FAngularD2 write FAngularD2;
  end;
implementation
{******************************************************************************}
// [15-9-2007]: TOXCBallAndSocketJoint last change by Dave Gravel.           //
{******************************************************************************}
constructor TOXCBallAndSocketJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 5;
  LocalUse:= False;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCBallAndSocketJoint last change by Dave Gravel.           //
{******************************************************************************}
procedure TOXCBallAndSocketJoint.CustomCreate( cBody, pBody: PNewtonBody; cPivot, cPin: TOXVector4 );
var
  matrix: TOXMatrix;
  Pin: TOXVector4;
begin
  FBody0:= cBody;
  FBody1:= pBody;
  // 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( cBody, @matrix[0, 0] );
  Pin:= oxVSubtract( oxV4Make( Matrix[0, 3], Matrix[1, 3], Matrix[2, 3], Matrix[3, 3] ), cPivot );
  if ( oxVDotProduct( Pin, Pin ) < 1.0e-3 ) then
    // the pivot is already at the origin then any pin will do
    Pin:= oxV4Make( Matrix[0, 0], Matrix[1, 0], Matrix[2, 0], Matrix[3, 0] );
    CalculateLocalMatrix( oxV3Make( cPivot ),
                          oxV3Make( Pin ),
                          FcLocalMatrix0, FcLocalMatrix1 );
  LocalMatrix0:= FcLocalMatrix0;
  LocalMatrix1:= FcLocalMatrix1;
end;
{******************************************************************************}
// [15-9-2007]: TOXCBallAndSocketJoint last change by Dave Gravel.           //
{******************************************************************************}
function TOXCBallAndSocketJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  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 ); 
  // Restrict the movement on the pivot point along all tree orthonormal direction
  AddLinearRow( @matrix0[3, 0], @matrix1[3, 0], @matrix1[0, 0] );
  AddLinearRow( @matrix0[3, 0], @matrix1[3, 0], @matrix1[1, 0] );
  AddLinearRow( @matrix0[3, 0], @matrix1[3, 0], @matrix1[2, 0] );
  result:= 0;
end;
{******************************************************************************}
// [15-9-2007]: TOXCConeLimitedBallAndSocketJoint last change by Dave Gravel.//
{******************************************************************************}
constructor TOXCConeLimitedBallAndSocketJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 5;
  LocalUse:= False;
  FConeMaxAngle:= 10;
  FCosConeAngle:= 0;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCConeLimitedBallAndSocketJoint last change by Dave Gravel.//
{******************************************************************************}
procedure TOXCConeLimitedBallAndSocketJoint.CustomCreate( cBody, pBody: PNewtonBody; cPivot, cPin: TOXVector4 );
begin
  FBody0:= cBody;
  FBody1:= pBody;
  // k00m I think here the Newton sdk have a little error.
  // I have reproduce the same thing because I don't know how to fix it.
  // Or the joint is just unfinished.
  // m_coneAngle = coneAngle;
  // m_coneAngle = twistAngle;
  // m_cosConeAngle = dCos (m_coneAngle);
  FCosConeAngle:= Cos( FConeMaxAngle * PI / 180 );
  // 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]: TOXCConeLimitedBallAndSocketJoint last change by Dave Gravel.//
{******************************************************************************}
function TOXCConeLimitedBallAndSocketJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  ConeCos: Float;
  MaxAngle: Float;
  p0: TOXVector3;
  p1: TOXVector3;
  LateralDir: TOXVector3;
  UnitLateralDir: TOXVector3;
  TangentDir: TOXVector3;
begin
  MaxAngle:= FConeMaxAngle * PI / 180;
  FCosConeAngle:= Cos( MaxAngle );
  // add the tree rows to keep the pivot in place
  // 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], @matrix1[0, 0] );
  AddLinearRow( @matrix0[3, 0], @matrix1[3, 0], @matrix1[1, 0] );
  AddLinearRow( @matrix0[3, 0], @matrix1[3, 0], @matrix1[2, 0] );
  // ///////////////////////////////////////////////////////////////////
  //
  // add a row to keep the child body inside the cone limit
  //
  // The child is inside the cone if the dCos of the angle between the pin and
  ConeCos:= oxVDotProduct( Matrix0[0], Matrix1[0] );
  if ConeCos < 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
    p0:= oxV3Make( oxVAdd( Matrix0[3], oxVScale( Matrix0[0], JointMinPinLength ) ));
    p1:= oxV3Make( oxVAdd( Matrix1[3], oxVScale( Matrix1[0], JointMinPinLength ) ));
    // get a vectors perpendicular to the plane of motion
    LateralDir:= oxV3Make( oxVCrossProduct( Matrix0[0], Matrix1[0] ));
    // note this could fail if the angle between matrix0.m_front and matrix1.m_front is 90 degree
    UnitLateralDir:= oxVScale( LateralDir, 1.0 / sqrt( oxVDotProduct( LateralDir, LateralDir ) ) );
    // 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
    AddLinearRow( @p0[0], @p0[0], @UnitLateralDir[0] );
    // calculate the unit vector tangent to the trajectory
    oxVCrossProduct( UnitLateralDir, oxV3Make( Matrix0[0] ), TangentDir );
    TangentDir:= oxVCrossProduct( UnitLateralDir, oxV3Make(Matrix0[0]) );
    p1:= oxVAdd( p0, oxVScale( oxVSubtract( p1, p0 ), 0.005 ) );
    AddLinearRow( @p0[0], @p1[0], @Tangentdir[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
    SetRowMinimumFriction( 0.0 );
  end;
  result:= 0;   
end;
{******************************************************************************}
// [15-9-2007]: TOXCCorkScrewJoint last change by Dave Gravel.               //
{******************************************************************************}
constructor TOXCCorkScrewJoint.Create( aOwner: TComponent );
begin
  inherited Create( aOwner );
  FOrionX3D:= 100;
  JointMinPinLength:= 50.0;
  MaxDOF:= 6;
  LocalUse:= False;
  FJointLimitsOn:= False;
  FJointMinDist:= -1.0;
  FJointMaxDist:= 1.0;
  FJointAngularMotorOn:= True;
  FJointAngularDamp:= 0.1;
  FJointAngularAccel:= 5.0;
  OnCustomCreate:= CustomCreate;
  OnCallBack:= SubmitConstrainst;
end;
{******************************************************************************}
// [15-9-2007]: TOXCCorkScrewJoint last change by Dave Gravel.               //
{******************************************************************************}
procedure TOXCCorkScrewJoint.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]: TOXCCorkScrewJoint last change by Dave Gravel.               //
{******************************************************************************}
function TOXCCorkScrewJoint.SubmitConstrainst( userJoint: PNewtonJoint ): Cardinal;
var
  matrix0: TOXMatrix;
  matrix1: TOXMatrix;
  RelOmega: Float;
  RelAccel: Float;
  Omega0: TOXVector3;
  Omega1: TOXVector3;
  p0: TOXVector3;
  p1: TOXVector3;
  q0: TOXVector3;
  q1: TOXVector3;
  dist: Float;
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:= oxV3Make(matrix0[3]);
  p1:= oxVAdd( oxV3Make(Matrix1[3]),
  oxVScale( oxV3Make(Matrix1[0]),
  oxVDotProduct( oxVSubtract( p0, oxV3Make(Matrix1[3]) ), oxV3Make(Matrix1[0]) ) ) );
  AddLinearRow( @p0[0], @p1[0], @Matrix0[1, 0] );
  AddLInearRow( @p0[0], @p1[0], @Matrix1[2, 0] );
  // Get a point along the pin axis at some reasonable large distance from the pivot
  q0:= oxVAdd( p0, oxVScale( oxV3Make(Matrix0[0]), JointMinPinLength ) );
  q1:= oxVAdd( p1, oxVScale( oxV3Make(Matrix1[0]), JointMinPinLength ) );
  // Two constraints row perpendiculars to the hinge pin
  AddLinearRow( @q0[0], @q1[0], @Matrix0[1, 0] );
  AddLinearRow( @q0[0], @q1[0], @Matrix0[2, 0] );
  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, 0], @Matrix0[3, 0], @Matrix0[0, 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, 0], @Matrix0[3, 0], @Matrix0[0, 0] );
      // allow the object to return but not to kick going forward
      SetRowMaximumFriction( 0.0 );
    end;
  end;
  if FJointAngularMotorOn then begin
    Omega0:= oxV3Make( 0, 0, 0 );
    Omega1:= oxV3Make( 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:= FJointAngularAccel - FJointAngularDamp * RelOmega;
    // if the motor capability is on, then set angular acceleration with zero angular correction
    AddAngularRow( 0.0, @Matrix0[0] );
    // override the angular acceleration for this Jacobian to the desired acceleration
    SetRowAcceleration( RelAccel );