geometry.pas

Delphi7编程80例(完全版)

end;

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

function CreateRotationMatrixX(Sine, Cosine: Single): TMatrix; register;

// create matrix for rotation about x-axis

begin
  Result:=EmptyMatrix;
  Result[0,0]:=1;
  Result[1,1]:=Cosine;
  Result[1,2]:=Sine;
  Result[2,1]:=-Sine;
  Result[2,2]:=Cosine;
  Result[3,3]:=1;
end;

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

function CreateRotationMatrixY(Sine, Cosine: Single): TMatrix; register;

// create matrix for rotation about y-axis

begin
  Result:=EmptyMatrix;
  Result[0,0]:=Cosine;
  Result[0,2]:=-Sine;
  Result[1,1]:=1;
  Result[2,0]:=Sine;
  Result[2,2]:=Cosine;
  Result[3,3]:=1;
end;

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

function CreateRotationMatrixZ(Sine, Cosine: Single): TMatrix; register;

// create matrix for rotation about z-axis

begin
  Result:=EmptyMatrix;
  Result[0,0]:=Cosine;
  Result[0,1]:=Sine;
  Result[1,0]:=-Sine;
  Result[1,1]:=Cosine;
  Result[2,2]:=1;
  Result[3,3]:=1;
end;

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

function CreateScaleMatrix(V: TAffineVector): TMatrix; register;

// create scaling matrix

begin
  Result:=EmptyMatrix;
  Result[X,X]:=V[X];
  Result[Y,Y]:=V[Y];
  Result[Z,Z]:=V[Z];
  Result[W,W]:=1;
end;

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

function CreateTranslationMatrix(V: TAffineVector): TMatrix; register;

// create translation matrix

begin
  Result:=IdentityMatrix;
  Result[W,X]:=V[X];
  Result[W,Y]:=V[Y];
  Result[W,Z]:=V[Z];
end;

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

function Lerp(Start,Stop,t: Single): Single;

// calculate linear interpolation between start and stop at point t

begin
  Result:=Start+(Stop-Start)*t;
end;

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

function VectorAffineLerp(V1,V2: TAffineVector; t: Single): TAffineVector;

// calculate linear interpolation between vector1 and vector2 at point t

begin
  Result[X]:=Lerp(V1[X],V2[X],t);
  Result[Y]:=Lerp(V1[Y],V2[Y],t);
  Result[Z]:=Lerp(V1[Z],V2[Z],t);
end;

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

function VectorLerp(V1,V2: TVector; t: Single): TVector;

// calculate linear interpolation between vector1 and vector2 at point t

begin
  Result[X]:=Lerp(V1[X],V2[X],t);
  Result[Y]:=Lerp(V1[Y],V2[Y],t);
  Result[Z]:=Lerp(V1[Z],V2[Z],t);
  Result[W]:=Lerp(V1[W],V2[W],t);
end;

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

function QuaternionSlerp(QStart,QEnd: TQuaternion; Spin: Integer; t: Single): TQuaternion;

// spherical linear interpolation of unit quaternions with spins
// QStart, QEnd - start and end unit quaternions
// t            - interpolation parameter (0 to 1)
// Spin         - number of extra spin rotations to involve

var beta,                   // complementary interp parameter
    theta,                  // angle between A and B
    sint, cost,             // sine, cosine of theta
    phi         : Single;   // theta plus spins
    bflip       : Boolean;  // use negation of B?


begin
  // cosine theta
  cost:=VectorAngle(QStart.Axis,QEnd.Axis);

  // if QEnd is on opposite hemisphere from QStart, use -QEnd instead
  if cost < 0 then
  begin
    cost:=-cost;
    bflip:=True;
  end
  else bflip:=False;

  // if QEnd is (within precision limits) the same as QStart,
  // just linear interpolate between QStart and QEnd.
  // Can't do spins, since we don't know what direction to spin.

  if (1-cost) < EPSILON then beta:=1-t
                    else
  begin
    // normal case
    theta:=arccos(cost);
    phi:=theta+Spin*Pi;
    sint:=sin(theta);
    beta:=sin(theta-t*phi)/sint;
    t:=sin(t*phi)/sint;
  end;

  if bflip then t:=-t;

  // interpolate
  Result.Vector[X]:=beta*QStart.Vector[X]+t*QEnd.Vector[X];
  Result.Vector[Y]:=beta*QStart.Vector[Y]+t*QEnd.Vector[Y];
  Result.Vector[Z]:=beta*QStart.Vector[Z]+t*QEnd.Vector[Z];
  Result.Angle:=beta*QStart.Angle+t*QEnd.Angle;
end;

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

function VectorAffineCombine(V1,V2: TAffineVector; F1,F2: Single): TAffineVector;

// make a linear combination of two vectors and return the result

begin
  Result[X]:=(F1*V1[X])+(F2*V2[X]);
  Result[Y]:=(F1*V1[Y])+(F2*V2[Y]);
  Result[Z]:=(F1*V1[Z])+(F2*V2[Z]);
end;

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

function VectorCombine(V1,V2: TVector; F1,F2: Single): TVector;

// make a linear combination of two vectors and return the result

begin
  Result[X]:=(F1*V1[X])+(F2*V2[X]);
  Result[Y]:=(F1*V1[Y])+(F2*V2[Y]);
  Result[Z]:=(F1*V1[Z])+(F2*V2[Z]);
  Result[W]:=(F1*V1[W])+(F2*V2[W]);
end;

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

function MatrixDecompose(M: TMatrix; var Tran: TTransformations): Boolean; register;

// Author: Spencer W. Thomas, University of Michigan
//
// MatrixDecompose - Decompose a non-degenerate 4x4 transformation matrix into
// the sequence of transformations that produced it.
//
// The coefficient of each transformation is returned in the corresponding
// element of the vector Tran.
//
// Returns true upon success, false if the matrix is singular.

var I,J      : Integer;
    LocMat,
    pmat,
    invpmat,
    tinvpmat : TMatrix;
    prhs,
    psol     : TVector;
    Row      : ARRAY[0..2] OF TAffineVector;

begin
  Result:=False;
  locmat:=M;
  // normalize the matrix
  if locmat[W,W] = 0 then Exit;
  for I:=0 to 3 do
    for J:=0 to 3 do
      locmat[I,J]:=locmat[I,J]/locmat[W,W];

  // pmat is used to solve for perspective, but it also provides
  // an easy way to test for singularity of the upper 3x3 component.

  pmat:=locmat;
  for I:=0 to 2 do pmat[I,W]:=0;
  pmat[W,W]:=1;

  if MatrixDeterminant(pmat) = 0 then Exit;

  // First, isolate perspective.  This is the messiest.
  if (locmat[X,W] <> 0) or
     (locmat[Y,W] <> 0) or
     (locmat[Z,W] <> 0) then
  begin
    // prhs is the right hand side of the equation.
    prhs[X]:=locmat[X,W];
    prhs[Y]:=locmat[Y,W];
    prhs[Z]:=locmat[Z,W];
    prhs[W]:=locmat[W,W];

    // Solve the equation by inverting pmat and multiplying
    // prhs by the inverse.  (This is the easiest way, not
    // necessarily the best.)

    invpmat:=pmat;
    MatrixInvert(invpmat);
    MatrixTranspose(invpmat);
    psol:=VectorTransform(prhs,tinvpmat);

    // stuff the answer away
    Tran[ttPerspectiveX]:=psol[X];
    Tran[ttPerspectiveY]:=psol[Y];
    Tran[ttPerspectiveZ]:=psol[Z];
    Tran[ttPerspectiveW]:=psol[W];

    // clear the perspective partition
    locmat[X,W]:=0;
    locmat[Y,W]:=0;
    locmat[Z,W]:=0;
    locmat[W,W]:=1;
  end
  else
  begin
    // no perspective
    Tran[ttPerspectiveX]:=0;
    Tran[ttPerspectiveY]:=0;
    Tran[ttPerspectiveZ]:=0;
    Tran[ttPerspectiveW]:=0;
  end;

  // next take care of translation (easy)
  for I:=0 to 2 do
  begin
    Tran[TTransType(Ord(ttTranslateX)+I)]:=locmat[W,I];
    locmat[W,I]:=0;
  end;

  // now get scale and shear
  for I:=0 to 2 do
  begin
    row[I,X]:=locmat[I,X];
    row[I,Y]:=locmat[I,Y];
    row[I,Z]:=locmat[I,Z];
  end;

  // compute X scale factor and normalize first row
  Tran[ttScaleX]:=Sqr(VectorNormalize(row[0])); // (ML) calc. optimized

  // compute XY shear factor and make 2nd row orthogonal to 1st
  Tran[ttShearXY]:=VectorAffineDotProduct(row[0],row[1]);
  row[1]:=VectorAffineCombine(row[1],row[0],1,-Tran[ttShearXY]);

  // now, compute Y scale and normalize 2nd row
  Tran[ttScaleY]:=Sqr(VectorNormalize(row[1])); // (ML) calc. optimized
  Tran[ttShearXY]:=Tran[ttShearXY]/Tran[ttScaleY];

  // compute XZ and YZ shears, orthogonalize 3rd row
  Tran[ttShearXZ]:=VectorAffineDotProduct(row[0],row[2]);
  row[2]:=VectorAffineCombine(row[2],row[0],1,-Tran[ttShearXZ]);
  Tran[ttShearYZ]:=VectorAffineDotProduct(row[1],row[2]);
  row[2]:=VectorAffineCombine(row[2],row[1],1,-Tran[ttShearYZ]);

  // next, get Z scale and normalize 3rd row
  Tran[ttScaleZ]:=Sqr(VectorNormalize(row[1])); // (ML) calc. optimized
  Tran[ttShearXZ]:=Tran[ttShearXZ]/tran[ttScaleZ];
  Tran[ttShearYZ]:=Tran[ttShearYZ]/Tran[ttScaleZ];

  // At this point, the matrix (in rows[]) is orthonormal.
  // Check for a coordinate system flip.  If the determinant
  // is -1, then negate the matrix and the scaling factors.
  if VectorAffineDotProduct(row[0],VectorCrossProduct(row[1],row[2])) < 0 then
    for I:=0 to 2 do
    begin
      Tran[TTransType(Ord(ttScaleX)+I)]:=-Tran[TTransType(Ord(ttScaleX)+I)];
      row[I,X]:=-row[I,X];
      row[I,Y]:=-row[I,Y];
      row[I,Z]:=-row[I,Z];
    end;

  // now, get the rotations out, as described in the gem
  Tran[ttRotateY]:=arcsin(-row[0,Z]);
  if cos(Tran[ttRotateY]) <> 0 then
  begin
    Tran[ttRotateX]:=arctan2(row[1,Z],row[2,Z]);
    Tran[ttRotateZ]:=arctan2(row[0,Y],row[0,X]);
  end
  else
  begin
    tran[ttRotateX]:=arctan2(row[1,X],row[1,Y]);
    tran[ttRotateZ]:=0;
  end;
  // All done!
  Result:=True;
end;

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

function VectorDblToFlt(V: THomogeneousDblVector): THomogeneousVector; register; assembler;

asm
              FLD  QWORD PTR [EAX]
              FSTP DWORD PTR [EDX]
              FLD  QWORD PTR [EAX+8]
              FSTP DWORD PTR [EDX+4]
              FLD  QWORD PTR [EAX+16]
              FSTP DWORD PTR [EDX+8]
              FLD  QWORD PTR [EAX+24]
              FSTP DWORD PTR [EDX+12]
end;

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

function VectorAffineDblToFlt(V: TAffineDblVector): TAffineVector; register; assembler;

asm
              FLD  QWORD PTR [EAX]
              FSTP DWORD PTR [EDX]
              FLD  QWORD PTR [EAX+8]
              FSTP DWORD PTR [EDX+4]
              FLD  QWORD PTR [EAX+16]
              FSTP DWORD PTR [EDX+8]
end;

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

function VectorAffineFltToDbl(V: TAffineVector): TAffineDblVector; register; assembler;

asm
              FLD  DWORD PTR [EAX]
              FSTP QWORD PTR [EDX]
              FLD  DWORD PTR [EAX+8]
              FSTP QWORD PTR [EDX+4]
              FLD  DWORD PTR [EAX+16]
              FSTP QWORD PTR [EDX+8]
end;

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

function VectorFltToDbl(V: TVector): THomogeneousDblVector; register; assembler;

asm
              FLD  DWORD PTR [EAX]
              FSTP QWORD PTR [EDX]
              FLD  DWORD PTR [EAX+8]
              FSTP QWORD PTR [EDX+4]
              FLD  DWORD PTR [EAX+16]
              FSTP QWORD PTR [EDX+8]
              FLD  DWORD PTR [EAX+24]
              FSTP QWORD PTR [EDX+12]
end;

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

end.