geometry.pas

一个用Delphi编写的很好的屏保程序

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

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

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

// creates matrix for rotation about x-axis

begin
  Result := EmptyMatrix;
  Result[X, X] := 1;
  Result[Y, Y] := Cosine;
  Result[Y, Z] := Sine;
  Result[Z, Y] := -Sine;
  Result[Z, Z] := Cosine;
  Result[W, W] := 1;
end;

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

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

// creates matrix for rotation about y-axis

begin
  Result := EmptyMatrix;
  Result[X, X] := Cosine;
  Result[X, Z] := -Sine;
  Result[Y, Y] := 1;
  Result[Z, X] := Sine;
  Result[Z, Z] := Cosine;
  Result[W, W] := 1;
end;

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

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

// creates matrix for rotation about z-axis

begin
  Result := EmptyMatrix;
  Result[X, X] := Cosine;
  Result[X, Y] := Sine;
  Result[Y, X] := -Sine;
  Result[Y, Y] := Cosine;
  Result[Z, Z] := 1;
  Result[W, W] := 1;
end;

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

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

// creates scaling matrix

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

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

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

// creates translation matrix

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

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

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

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

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

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

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

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

// calculates 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 negativ t?


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

  // 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.ImagPart[X] := beta * QStart.ImagPart[X] + t * QEnd.ImagPart[X];
  Result.ImagPart[Y] := beta * QStart.ImagPart[Y] + t * QEnd.ImagPart[Y];
  Result.ImagPart[Z] := beta * QStart.ImagPart[Z] + t * QEnd.ImagPart[Z];
  Result.RealPart := beta * QStart.RealPart + t * QEnd.RealPart;
end;

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

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

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

// makes 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-degenerated 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: calculation 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: calculation 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; assembler;

// converts a vector containing double sized values into a vector with single sized values

asm
              FLD  QWORD