geometry.pas

Delphi7编程80例(完全版)

function VectorAffineDotProduct(V1,V2: TAffineVector): Single; register;

begin
  Result:=V1[X]*V2[X]+V1[Y]*V2[Y]+V1[Z]*V2[Z];
end;

//------------------------------------------------------------------------------

function VectorCrossProduct(V1,V2: TAffineVector): TAffineVector; register;

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(V, N: TAffineVector): TAffineVector; register; 

// 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(V: TVector; M: TMatrix): TVector; register;

// 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(V: TAffineVector; M: TAffineMatrix): TAffineVector; register;

// 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(M: TAffineMatrix): Single; register;

// 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); register;

// 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(M: TMatrix): Single; register;

// 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; Factor: Single); register;

// 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); register;

// 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); register;

// 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); register;

// 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(M1, M2: TMatrix): TMatrix; register;

// 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(Axis: TAffineVector; Angle: Single): TMatrix; register; 

// 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;

//------------------------------------------------------------------------------

function ConvertRotation(Angles: TAffineVector): TVector; register;

{ 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]);
  M2:=CreateRotationMatrix(Axis2,Angles[Y]);
  M1:=MatrixMultiply(M,M2);

  Axis2:=MakeAffineVector([0,0,1]);
  M2:=CreateRotationMatrix(Axis2,Angles[Z]);
  M:=MatrixMultiply(M1,M2);

  cost:=((M[X,X]+M[Y,Y]+M[Z,Z])-1)/2;
  if cost < -1 then cost:=-1
               else
    if cost > 1-EPSILON then
    begin
      // Bad angle - this would cause a crash
      Result:=MakeVector([1,0,0,0]);
      Exit;
    end;

  cost1:=1-cost;
  Result:=Makevector([Sqrt((M[X,X]-cost)/cost1),
                      Sqrt((M[Y,Y]-cost)/cost1),
                      sqrt((M[Z,Z]-cost)/cost1),
                      arccos(cost)]);

  sint:=2*Sqrt(1-cost*cost); // This is actually 2 Sin(t)

  // Determine the proper signs
  for I:=0 to 7 do
  begin
    if (I and 1) > 1 then s1:=-1 else s1:=1;
    if (I and 2) > 1 then s2:=-1 else s2:=1;
    if (I and 4) > 1 then s3:=-1 else s3:=1;
    if (Abs(s1*Result[X]*sint-M[Y,Z]+M[Z,Y]) < EPSILON2) and
       (Abs(s2*Result[Y]*sint-M[Z,X]+M[X,Z]) < EPSILON2) and
       (Abs(s3*Result[Z]*sint-M[X,Y]+M[Y,X]) < EPSILON2) then
        begin
          // We found the right combination of signs
          Result[X]:=Result[X]*s1;
          Result[Y]:=Result[Y]*s2;
          Result[Z]:=Result[Z]*s3;
          Exit;
        end;
  end;