newtoncustomjoints_math.pas

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

{*******************************************************************************}
{                                                                               }
{      Math helper unit for NewtonCustomJoints.pas                              }
{                                                                               }
{      Copyright (c) 2005,06 Sascha Willems                                     }
{                                                                               }
{*******************************************************************************}
{                                                                               }
{ License :                                                                     }
{                                                                               }
{  The contents of this file are used with permission, subject to               }
{  the Mozilla Public License Version 1.1 (the "License"); you may              }
{  not use this file except in compliance with the License. You may             }
{  obtain a copy of the License at                                              }
{  http://www.mozilla.org/MPL/MPL-1.1.html                                      }
{                                                                               }
{  Software distributed under the License is distributed on an                  }
{  "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or               }
{  implied. See the License for the specific language governing                 }
{  rights and limitations under the License.                                    }
{                                                                               }
{*******************************************************************************}

unit NewtonCustomJoints_Math;

interface

uses
 Math;

type
 TMatrix4f = array[0..3, 0..3] of Single;
 TMatrix4d = array[0..3, 0..3] of Double;
 TVector3f = array[0..2] of Single;
 TVector4f = array[0..3] of Single;

var
 NullMatrix4f   : TMatrix4f = ((0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 0));
 IdentityMatrix : TMatrix4f = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1));
 ZeroVector     : TVector3f = (0, 0, 0);

const
  X = 0;
  Y = 1;
  Z = 2;
  W = 3;
  Epsilon = 0.0001;

function  Matrix_Multiply(m1 : TMatrix4f; m2 : TMatrix4f) : TMatrix4f;
procedure Matrix_SetIdentity(var M : TMatrix4f);
procedure Matrix_SetTransform(var M : TMatrix4f; V : TVector3f);
procedure Matrix_SetRotation(var M : TMatrix4f; V : TVector3f);
procedure Matrix_RotateVect(const M : TMatrix4f; var pVect : TVector3f);
procedure Matrix_Inverse(var M: TMatrix4f);
function  Matrix_TansformVector(const M : TMatrix4f; V : TVector3f) : TVector3f;
function  Matrix_UntransformVector(const M : TMatrix4f; V : TVector3f) : TVector3f;
function  Matrix_UnRotateVect(const M : TMatrix4f; pVect : TVector3f) : TVector3f;
procedure Matrix_SetColumn(var M : TMatrix4f; pColumn : Byte;pVect : TVector4f);


function V4(x,y,z : Single) : TVector4f; overload;
function V4(x,y,z,w : Single) : TVector4f; overload;
function V4(pV3 : TVector3f; w : Single) : TVector4f; overload;
function V4(pMatrix : TMatrix4f; pC : Integer) : TVector4f; overload;
function V3(x,y,z : Single) : TVector3f; overload;
function V3(pMatrix : TMatrix4f; pC : Integer) : TVector3f; overload;
function VCross(pV1, pV2 : TVector3f) : TVector3f; overload;
function VCross(pV1, pV2 : TVector4f) : TVector4f; overload;
function VDot(pV1, pV2 : TVector3f) : Single; overload;
function VDot(pV1, pV2 : TVector4f) : Single; overload;
function VTransform(pV1 : TVector3f; pM : TMatrix4f) : TVector3f;
function VSub(pV1, pV2 : TVector3f) : TVector3f; overload;
function VSub(pV1, pV2 : TVector4f) : TVector4f; overload;
function VAdd(pV1, pV2 : TVector3f) : TVector3f; overload;
function VAdd(pV1, pV2 : TVector4f) : TVector4f; overload;
function VNormalize(pV : TVector3f) : TVector3f;
function VScale(pV : TVector3f; pScale : Single) : TVector3f; overload;
function VScale(pV : TVector4f; pScale : Single) : TVector4f; overload;
function VDistance(pV1, pV2 : TVector3f) : Single;

implementation

// =============================================================================
//  V4
// =============================================================================
function V4(x,y,z : Single) : TVector4f;
begin
Result[0] := x;
Result[1] := y;
Result[2] := z;
Result[3] := 0;
end;

function V4(x,y,z,w : Single) : TVector4f;
begin
Result[0] := x;
Result[1] := y;
Result[2] := z;
Result[3] := w;
end;

function V4(pV3 : TVector3f; w : Single) : TVector4f; overload;
begin
Result[0] := pV3[0];
Result[1] := pV3[1];
Result[2] := pV3[2];
Result[3] := w;
end;

function V4(pMatrix : TMatrix4f; pC : Integer) : TVector4f; overload;
begin
Result[0] := pMatrix[pC, 0];
Result[1] := pMatrix[pC, 1];
Result[2] := pMatrix[pC, 2];
Result[3] := pMatrix[pC, 3];
end;


// =============================================================================
//  V3
// =============================================================================
function V3(x,y,z : Single) : TVector3f;
begin
Result[0] := x;
Result[1] := y;
Result[2] := z;
end;

function V3(pMatrix : TMatrix4f; pC : Integer) : TVector3f; overload;
begin
Result[0] := pMatrix[pC, 0];
Result[1] := pMatrix[pC, 1];
Result[2] := pMatrix[pC, 2];
end;

// =============================================================================
//  VCross
// =============================================================================
function VCross(pV1, pV2 : TVector3f) : TVector3f;
begin
Result[0] := (pV1[1]*pV2[2]) - (pV1[2]*pV2[1]);
Result[1] := (pV1[2]*pV2[0]) - (pV1[0]*pV2[2]);
Result[2] := (pV1[0]*pV2[1]) - (pV1[1]*pV2[0]);
end;

function VCross(pV1, pV2 : TVector4f) : TVector4f;
begin
Result[0] := (pV1[1]*pV2[2]) - (pV1[2]*pV2[1]);
Result[1] := (pV1[2]*pV2[0]) - (pV1[0]*pV2[2]);
Result[2] := (pV1[0]*pV2[1]) - (pV1[1]*pV2[0]);
Result[3] := 0;
end;

// =============================================================================
//  VDot
// =============================================================================
function VDot(pV1, pV2 : TVector3f) : Single;
begin
Result := (pV1[0]*pV2[0]) + (pV1[1]*pV2[1]) + (pV1[2]*pV2[2]);
end;

function VDot(pV1, pV2 : TVector4f) : Single;
begin
Result := (pV1[0]*pV2[0]) + (pV1[1]*pV2[1]) + (pV1[2]*pV2[2]) + (pV1[3]*pV2[3]);
end;

// =============================================================================
//  VTransform
// =============================================================================
function VTransform(pV1 : TVector3f; pM : TMatrix4f) : TVector3f;
var
 TV : TVector3f;
begin
TV[X] := pV1[X] * pM[X, X] + pV1[Y] * pM[Y, X] + pV1[Z] * pM[Z, X] + pM[W, X];
TV[Y] := pV1[X] * pM[X, Y] + pV1[Y] * pM[Y, Y] + pV1[Z] * pM[Z, Y] + pM[W, Y];
TV[Z] := pV1[X] * pM[X, Z] + pV1[Y] * pM[Y, Z] + pV1[Z] * pM[Z, Z] + pM[W, Z];
Result := TV
end;

// =============================================================================
//  VSub
// =============================================================================
function VSub(pV1, pV2 : TVector3f) : TVector3f;
begin
Result := V3(pV1[0]-pV2[0], pV1[1]-pV2[1], pV1[2]-pV2[2]);
end;

function VSub(pV1, pV2 : TVector4f) : TVector4f; overload;
begin
Result := V4(pV1[0]-pV2[0], pV1[1]-pV2[1], pV1[2]-pV2[2], pV1[3]-pV2[3]);
end;

// =============================================================================
//  VAdd
// =============================================================================
function VAdd(pV1, pV2 : TVector3f) : TVector3f;
begin
Result := V3(pV1[0]+pV2[0], pV1[1]+pV2[1], pV1[2]+pV2[2]);
end;

function VAdd(pV1, pV2 : TVector4f) : TVector4f; overload;
begin
Result := V4(pV1[0]+pV2[0], pV1[1]+pV2[1], pV1[2]+pV2[2], pV1[3]+pV2[3]);
end;


// =============================================================================
//  VNormalize
// =============================================================================
function VNormalize(pV : TVector3f) : TVector3f;
var
 l : Single;
begin
l := Sqrt(pV[0]*pV[0] + pV[1]*pV[1] + pV[2]*pV[2]);
if l = 0 then
 exit;
Result[0] := pV[0]/l;
Result[1] := pV[1]/l;
Result[2] := pV[2]/l;
end;

// =============================================================================
//  VScale
// =============================================================================
function VScale(pV : TVector3f; pScale : Single) : TVector3f;
begin
Result[0] := pV[0] * pScale;
Result[1] := pV[1] * pScale;
Result[2] := pV[2] * pScale;
end;

function VScale(pV : TVector4f; pScale : Single) : TVector4f; overload;
begin