teegeometry.pas

BCB第三方组件

  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;
end;
{$ENDIF}

function CreateRotationMatrixX(const Sine, Cosine: Single): TMatrix;
// 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(const Sine, Cosine: Single): TMatrix;
// 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(const Sine, Cosine: Single): TMatrix;
// 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(const V: TAffineVector): TMatrix;
// 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(const V: TAffineVector): TMatrix;
// 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(const V1,V2: TAffineVector; const 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(const V1,V2: TVector; const 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;

{$IFNDEF TEENOASM}
function QuaternionSlerp(const 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.
  // Cant do spins, since we dont 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;
{$ENDIF}

function VectorAffineCombine(const V1,V2: TAffineVector; const 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(const V1,V2: TVector; const 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;

{$IFNDEF TEENOASM}
function MatrixDecompose(const M: TMatrix; var Tran: TTransformations): Boolean;

// 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(const V: THomogeneousDblVector): THomogeneousVector; 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(const V: TAffineDblVector): TAffineVector; 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(const V: TAffineVector): TAffineDblVector; 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(const V: TVector): THomogeneousDblVector; 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;
{$ENDIF}

end.