teegeometry.pas

BCB第三方组件

{$ENDIF}

function VectorAffineSubtract(const V1,V2: TAffineVector): TAffineVector;
// return the difference of v1 minus v2
begin
  Result[X]:=V1[X]-V2[X];
  Result[Y]:=V1[Y]-V2[Y];
  Result[Z]:=V1[Z]-V2[Z];
end;

function VectorReflect(const V, N: TAffineVector): TAffineVector;
// reflect vector V against N (assumes N is normalized)
var Dot : Single;
begin
   Dot:=VectorAffineDotProduct(V,N);
   Result[X]:=V[X]-2*Dot*N[X];
   Result[Y]:=V[Y]-2*Dot*N[Y];
   Result[Z]:=V[Z]-2*Dot*N[Z];
end;


{$IFNDEF TEENOASM}
procedure VectorScale(const V: array of Single; const Factor: Single); assembler;
// return a vector scaled by a factor
// EAX contains address of V
// EDX contains highest index in V
// Factor is located on the stack (don't ask me why not in ECX)

asm
  {for I:=Low(V) to High(V) do V[I]:=V[I]*Factor;}
              FLD DWORD PTR [Factor]        // load factor
@@Loop:       FLD DWORD PTR [EAX+4*EDX]     // load a component
              FMUL ST,ST(1)                 // multiply it with the factor
              WAIT
              FSTP DWORD PTR [EAX+4*EDX]    // store the result
              DEC EDX                       // do the entire array
              JNS @@Loop
              FSTP ST(0)                    // clean the FPU stack
end;



procedure VectorNegate(const V: array of Single); assembler;
// return a negated vector
// EAX contains address of V
// EDX contains highest index in V
asm
  {V[X]:=-V[X];
  V[Y]:=-V[Y];
  V[Z]:=-V[Z];}
@@Loop:       FLD DWORD PTR [EAX+4*EDX]
              FCHS
              WAIT
              FSTP DWORD PTR [EAX+4*EDX]
              DEC EDX
              JNS @@Loop
end;


{$ENDIF}

function VectorAdd(const V1,V2: TVector): TVector;
// return the sum of two vectors
begin
  Result[X]:=V1[X]+V2[X];
  Result[Y]:=V1[Y]+V2[Y];
  Result[Z]:=V1[Z]+V2[Z];
  Result[W]:=V1[W]+V2[W];
end;

function VectorAffineAdd(const V1,V2: TAffineVector): TAffineVector;
// return the sum of two vectors
begin
  Result[X]:=V1[X]+V2[X];
  Result[Y]:=V1[Y]+V2[Y];
  Result[Z]:=V1[Z]+V2[Z];
end;

function VectorSubtract(const V1,V2: TVector): TVector;
// return the difference of two vectors
begin
  Result[X]:=V1[X]-V2[X];
  Result[Y]:=V1[Y]-V2[Y];
  Result[Z]:=V1[Z]-V2[Z];
  Result[W]:=V1[W]-V2[W];
end;



function VectorDotProduct(const V1,V2: TVector): Single;
begin
  Result:=V1[X]*V2[X]+V1[Y]*V2[Y]+V1[Z]*V2[Z]+V1[W]*V2[W];
end;

function VectorAffineDotProduct(const V1,V2: TAffineVector): Single;
begin
  Result:=V1[X]*V2[X]+V1[Y]*V2[Y]+V1[Z]*V2[Z];
end;

function VectorCrossProduct(const V1,V2: TAffineVector): TAffineVector;
begin
  Result[X]:=V1[Y]*V2[Z]-V1[Z]*V2[Y];
  Result[Y]:=V1[Z]*V2[X]-V1[X]*V2[Z];
  Result[Z]:=V1[X]*V2[Y]-V1[Y]*V2[X];
end;

function VectorPerpendicular(const V, N: TAffineVector): TAffineVector;
// calculate a vector perpendicular to N (N is assumed to be of unit length)
// subtract out any component parallel to N
var Dot : Single;
begin
   Dot:=VectorAffineDotProduct(V,N);
   Result[X]:=V[X]-Dot*N[X];
   Result[Y]:=V[Y]-Dot*N[Y];
   Result[Z]:=V[Z]-Dot*N[Z];
end;

function VectorTransform(const V: TVector; const M: TMatrix): TVector;
// transform a homogeneous vector by multiplying it with a matrix
var TV : TVector;
begin
  TV[X]:=V[X]*M[X,X]+V[Y]*M[Y,X]+V[Z]*M[Z,X]+V[W]*M[W,X];
  TV[Y]:=V[X]*M[X,Y]+V[Y]*M[Y,Y]+V[Z]*M[Z,Y]+V[W]*M[W,Y];
  TV[Z]:=V[X]*M[X,Z]+V[Y]*M[Y,Z]+V[Z]*M[Z,Z]+V[W]*M[W,Z];
  TV[W]:=V[X]*M[X,W]+V[Y]*M[Y,W]+V[Z]*M[Z,W]+V[W]*M[W,W];
  Result:=TV
end;

function VectorAffineTransform(const V: TAffineVector; const M: TAffineMatrix): TAffineVector;
// transform an affine vector by multiplying it with a matrix
var TV : TAffineVector;
begin
  TV[X]:=V[X]*M[X,X]+V[Y]*M[Y,X]+V[Z]*M[Z,X];
  TV[Y]:=V[X]*M[X,Y]+V[Y]*M[Y,Y]+V[Z]*M[Z,Y];
  TV[Z]:=V[X]*M[X,Z]+V[Y]*M[Y,Z]+V[Z]*M[Z,Z];
  Result:=TV;
end;

function MatrixAffineDeterminant(const M: TAffineMatrix): Single;
// determinant of a 3x3 matrix
begin
  Result:=M[X,X]*(M[Y,Y]*M[Z,Z]-M[Z,Y]*M[Y,Z])-
          M[X,Y]*(M[Y,X]*M[Z,Z]-M[Z,X]*M[Y,Z])+
	  M[X,Z]*(M[Y,X]*M[Z,Y]-M[Z,X]*M[Y,Y]);
end;

function MatrixDetInternal(a1,a2,a3,b1,b2,b3,c1,c2,c3: Single): Single;
// internal version for the determinant of a 3x3 matrix
begin
  result:= a1 * (b2 * c3 - b3 * c2) -
           b1 * (a2 * c3 - a3 * c2) +
           c1 * (a2 * b3 - a3 * b2);
end;

procedure MatrixAdjoint(var M: TMatrix);
// Adjoint of a 4x4 matrix - used in the computation of the inverse
// of a 4x4 matrix
var a1,a2,a3,a4,
    b1,b2,b3,b4,
    c1,c2,c3,c4,
    d1,d2,d3,d4  : Single;
begin
    a1:= M[0,0]; b1:= M[0,1];
    c1:= M[0,2]; d1:= M[0,3];
    a2:= M[1,0]; b2:= M[1,1];
    c2:= M[1,2]; d2:= M[1,3];
    a3:= M[2,0]; b3:= M[2,1];
    c3:= M[2,2]; d3:= M[2,3];
    a4:= M[3,0]; b4:= M[3,1];
    c4:= M[3,2]; d4:= M[3,3];

    // row column labeling reversed since we transpose rows & columns
    M[X,X]:= MatrixDetInternal(b2,b3,b4,c2,c3,c4,d2,d3,d4);
    M[Y,X]:=-MatrixDetInternal(a2,a3,a4,c2,c3,c4,d2,d3,d4);
    M[Z,X]:= MatrixDetInternal(a2,a3,a4,b2,b3,b4,d2,d3,d4);
    M[W,X]:=-MatrixDetInternal(a2,a3,a4,b2,b3,b4,c2,c3,c4);

    M[X,Y]:=-MatrixDetInternal(b1,b3,b4,c1,c3,c4,d1,d3,d4);
    M[Y,Y]:= MatrixDetInternal(a1,a3,a4,c1,c3,c4,d1,d3,d4);
    M[Z,Y]:=-MatrixDetInternal(a1,a3,a4,b1,b3,b4,d1,d3,d4);
    M[W,Y]:= MatrixDetInternal(a1,a3,a4,b1,b3,b4,c1,c3,c4);

    M[X,Z]:= MatrixDetInternal(b1,b2,b4,c1,c2,c4,d1,d2,d4);
    M[Y,Z]:=-MatrixDetInternal(a1,a2,a4,c1,c2,c4,d1,d2,d4);
    M[Z,Z]:= MatrixDetInternal(a1,a2,a4,b1,b2,b4,d1,d2,d4);
    M[W,Z]:=-MatrixDetInternal(a1,a2,a4,b1,b2,b4,c1,c2,c4);

    M[X,W]:=-MatrixDetInternal(b1,b2,b3,c1,c2,c3,d1,d2,d3);
    M[Y,W]:= MatrixDetInternal(a1,a2,a3,c1,c2,c3,d1,d2,d3);
    M[Z,W]:=-MatrixDetInternal(a1,a2,a3,b1,b2,b3,d1,d2,d3);
    M[W,W]:= MatrixDetInternal(a1,a2,a3,b1,b2,b3,c1,c2,c3);
end;

function MatrixDeterminant(const M: TMatrix): Single;
// Determinant of a 4x4 matrix
var a1, a2, a3, a4,
    b1, b2, b3, b4,
    c1, c2, c3, c4,
    d1, d2, d3, d4  : Single;
begin
  a1:=M[X,X]; b1:=M[X,Y]; c1:=M[X,Z]; d1:=M[X,W];
  a2:=M[Y,X]; b2:=M[Y,Y]; c2:=M[Y,Z]; d2:=M[Y,W];
  a3:=M[Z,X]; b3:=M[Z,Y]; c3:=M[Z,Z]; d3:=M[Z,W];
  a4:=M[W,X]; b4:=M[W,Y]; c4:=M[W,Z]; d4:=M[W,W];

  Result:=a1*MatrixDetInternal(b2,b3,b4,c2,c3,c4,d2,d3,d4)-
          b1*MatrixDetInternal(a2,a3,a4,c2,c3,c4,d2,d3,d4)+
          c1*MatrixDetInternal(a2,a3,a4,b2,b3,b4,d2,d3,d4)-
          d1*MatrixDetInternal(a2,a3,a4,b2,b3,b4,c2,c3,c4);
end;



procedure MatrixScale(var M: TMatrix; const Factor: Single);
// multiply all elements of a 4x4 matrix with a factor
var I,J : Integer;
begin
  for I:=0 to 3 do
    for J:=0 to 3 do M[I,J]:=M[I,J]*Factor;
end;

procedure MatrixInvert(var M: TMatrix);
// find the inverse of a 4x4 matrix
var Det : Single;
begin
  Det:=MatrixDeterminant(M);
  if Abs(Det) < EPSILON then M:=IdentityMatrix
                        else
  begin
    MatrixAdjoint(M);
    MatrixScale(M,1/Det);
  end;
end;

procedure MatrixTranspose(var M: TMatrix);
// compute transpose of 4x4 matrix
var I,J : Integer;
    TM  : TMatrix;
begin
  for I:=0 to 3 do
    for J:=0 to 3 do TM[J,I]:=M[I,J];
  M:=TM;
end;

procedure MatrixAffineTranspose(var M: TAffineMatrix);
// compute transpose of 3x3 matrix
var I,J : Integer;
    TM  : TAffineMatrix;
begin
  for I:=0 to 2 do
    for J:=0 to 2 do TM[J,I]:=M[I,J];
  M:=TM;
end;

function MatrixMultiply(const M1, M2: TMatrix): TMatrix;
// multiply two 4x4 matrices
var I,J : Integer;
    TM  : TMatrix;
begin
  for I:=0 to 3 do
    for J:=0 to 3 do
      TM[I,J]:=M1[I,X]*M2[X,J]+
               M1[I,Y]*M2[Y,J]+
               M1[I,Z]*M2[Z,J]+
               M1[I,W]*M2[W,J];
  Result:=TM;
end;

function CreateRotationMatrix(const Axis: TAffineVector; const Angle: Single): TMatrix;
// Create a rotation matrix along the given axis by the given angle in radians.
// The axis is a set of direction cosines.
var cosine, sine,
    one_minus_cosine : Extended;
begin
    SinCos(Angle,Sine,Cosine);
    one_minus_cosine:=1-cosine;

    Result[X,X]:=SQR(Axis[X])+(1-SQR(axis[X]))*cosine;
    Result[X,Y]:=Axis[X]*Axis[Y]*one_minus_cosine+Axis[Z]*sine;
    Result[X,Z]:=Axis[X]*Axis[Z]*one_minus_cosine-Axis[Y]*sine;
    Result[X,W]:=0;

    Result[Y,X]:=Axis[X]*Axis[Y]*one_minus_cosine-Axis[Z]*sine;
    Result[Y,Y]:=SQR(Axis[Y])+(1-SQR(axis[Y]))*cosine;
    Result[Y,Z]:=Axis[Y]*Axis[Z]*one_minus_cosine+Axis[X]*sine;
    Result[Y,W]:=0;

    Result[Z,X]:=Axis[X]*Axis[Z]*one_minus_cosine+Axis[Y]*sine;
    Result[Z,Y]:=Axis[Y]*Axis[Z]*one_minus_cosine-Axis[X]*sine;
    Result[Z,Z]:=SQR(Axis[Z])+(1-SQR(axis[Z]))*cosine;
    Result[Z,W]:=0;

    Result[W,X]:=0;
    Result[W,Y]:=0;
    Result[W,Z]:=0;
    Result[W,W]:=1;
end;

{$IFNDEF TEENOASM}
function ConvertRotation(const Angles: TAffineVector): TVector;

{ Turn a triplet of rotations about x, y, and z (in that order) into an
   equivalent rotation around a single axis (all in radians).

   Rotation of the angle t about the axis (X,Y,Z) is given by:

     | X^2+(1-X^2) Cos(t),    XY(1-Cos(t)) + Z Sin(t), XZ(1-Cos(t))-Y Sin(t) |
 M = | XY(1-Cos(t))-Z Sin(t), Y^2+(1-Y^2) Cos(t),      YZ(1-Cos(t))+X Sin(t) |
     | XZ(1-Cos(t))+Y Sin(t), YZ(1-Cos(t))-X Sin(t),   Z^2+(1-Z^2) Cos(t)    |

   Rotation about the three axes (angles a1, a2, a3) can be represented as
   the product of the individual rotation matrices:

      | 1  0       0       | | Cos(a2) 0 -Sin(a2) | |  Cos(a3) Sin(a3) 0 |
      | 0  Cos(a1) Sin(a1) |*| 0       1  0       |*| -Sin(a3) Cos(a3) 0 |
      | 0 -Sin(a1) Cos(a1) | | Sin(a2) 0  Cos(a2) | |  0       0       1 |
	     Mx                       My                     Mz

   We now want to solve for X, Y, Z, and t given 9 equations in 4 unknowns.
   Using the diagonal elements of the two matrices, we get:

      X^2+(1-X^2) Cos(t) = M[0][0]
      Y^2+(1-Y^2) Cos(t) = M[1][1]
      Z^2+(1-Z^2) Cos(t) = M[2][2]

   Adding the three equations, we get:

      X^2 + Y^2 + Z^2 - (M[0][0] + M[1][1] + M[2][2]) =
	 - (3 - X^2 - Y^2 - Z^2) Cos(t)

   Since (X^2 + Y^2 + Z^2) = 1, we can rewrite as:

      Cos(t) = (1 - (M[0][0] + M[1][1] + M[2][2])) / 2

   Solving for t, we get:

      t = Acos(((M[0][0] + M[1][1] + M[2][2]) - 1) / 2)

    We can substitute t into the equations for X^2, Y^2, and Z^2 above
    to get the values for X, Y, and Z.  To find the proper signs we note
    that:

	2 X Sin(t) = M[1][2] - M[2][1]
	2 Y Sin(t) = M[2][0] - M[0][2]
	2 Z Sin(t) = M[0][1] - M[1][0]
}

var Axis1, Axis2 : TAffineVector;
    M, M1, M2    : TMatrix;
    cost, cost1,
    sint,
    s1, s2, s3   : Single;
    I            : Integer;
begin
  // see if we are only rotating about a single axis
  if Abs(Angles[X]) < EPSILON then
  begin
    if Abs(Angles[Y]) < EPSILON then
    begin
      Result:=MakeVector([0,0,1,Angles[Z]]);
      Exit;
    end
    else
      if Abs(Angles[Z]) < EPSILON then
      begin
        Result:=MakeVector([0,1,0,Angles[Y]]);
        Exit;
      end
   end
   else
     if (Abs(Angles[Y]) < EPSILON) and
        (Abs(Angles[Z]) < EPSILON) then
     begin
       Result:=MakeVector([1,0,0,Angles[X]]);
       Exit;
     end;

  // make the rotation matrix
  Axis1:=MakeAffineVector([1,0,0]);
  M:=CreateRotationMatrix(Axis1,Angles[X]);

  Axis2:=MakeAffineVector([0,1,0]);