geometry.pas

Delphi7编程80例(完全版)

// calculates the length of a vector following the equation: sqrt(x*x+y*y+...)
// Note: The parameter of this function is declared as open array. Thus
// there's no restriction about the number of the components of the vector.
//
// EAX contains the pointer to the data and EDX the number of array members.
// The result is returned in ST(0) and will be automatically converted, if a
// non-Single value is needed.

asm
              FLDZ                          // initialize sum
@@Loop:       FLD  DWORD PTR [EAX+4*EDX]    // load a component
              FMUL ST,ST
              FADDP
              SUB  EDX,1
              JNL  @@Loop
              FSQRT
end;

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

function VectorAngle(V1,V2: TAffineVector): Single; register; assembler;

// calculates the cosine of the angle between Vector1 and Vector2
// Result = DotProduct(V1,V2) / (Length(V1)*Length(V2))
//
// EAX contains Address of Vector1
// EDX contains Address of Vector2

asm
              FLD DWORD PTR [EAX]           // V1[0]
              FLD ST                        // Single V1[0]
              FMUL ST,ST                    // V1[0]**2 (prep. for divisor)
              FLD DWORD PTR [EDX]           // V2[0]
              FMUL ST(2),ST                 // ST(2):=V1[0]*V2[0]
              FMUL ST,ST                    // V2[0]**2 (prep. for divisor)
              FLD DWORD PTR [EAX+4]         // V1[1]
              FLD ST                        // Single V1[1]
              FMUL ST,ST                    // ST(0):=V1[1]**2
              FADDP ST(3),ST                // ST(2):=V1[0]**2+V1[1]**2
              FLD DWORD PTR [EDX+4]         // V2[1]
              FMUL ST(1),ST                 // ST(1):=V1[1]*V2[1]
              FMUL ST,ST                    // ST(0):=V2[1]**2
              FADDP ST(2),ST                // ST(1):=V2[0]**2+V2[1]**2
              FADDP ST(3),ST                // ST(2):=V1[0]*V2[0]+V1[1]*V2[1]
              FLD DWORD PTR [EAX+8]         // load V2[1]
              FLD ST                        // same calcs go here
              FMUL ST,ST                    // (compare above)
              FADDP ST(3),ST
              FLD DWORD PTR [EDX+8]
              FMUL ST(1),ST
              FMUL ST,ST
              FADDP ST(2),ST
              FADDP ST(3),ST
              FMULP                         // ST(0):=(V1[0]**2+V1[1]**2+V1[2])*
                                            //        (V2[0]**2+V2[1]**2+V2[2])
              FSQRT                         // sqrt(ST(0))
              FDIVP                         // ST(0)=Result:=ST(1)/ST(0)
  // the result is expected in ST(0), if it's invalid, an error is raised
end;

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

function PointInPolygon(xp, yp : array of Single; x,y: Single): Boolean;

// The code below is from Wm. Randolph Franklin <[e-mail]>
// with some minor modifications for speed.  It returns 1 for strictly
// interior points, 0 for strictly exterior, and 0 or 1 for points on
// the boundary.

var I, J : Integer;

begin
  Result:=False;
  if High(XP) <> High(YP) then Exit;
  J:=High(XP);
  for I:=0 to High(XP) do
  begin
    if ((((yp[I]<=y) and (y<yp[J])) OR ((yp[J]<=y) and (y<yp[I]))) and
        (x < (xp[J]-xp[I])*(y-yp[I])/(yp[J]-yp[I])+xp[I]))
    then Result:=not Result;
    J:=I+1;
  end;
end;

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

function QuaternionConjugate(Q: TQuaternion): TQuaternion; register; assembler;

// return the conjugate of a quaternion
// EAX contains address of Q
// EDX contains address of result

asm
              FLD DWORD PTR [EAX]
              FCHS
              WAIT
              FSTP DWORD PTR [EDX]
              FLD DWORD PTR [EAX+4]
              FCHS
              WAIT
              FSTP DWORD PTR [EDX+4]
              FLD DWORD PTR [EAX+8]
              FCHS
              WAIT
              FSTP DWORD PTR [EDX+8]
              MOV EAX,[EAX+12]
              MOV [EDX+12],EAX
end;

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

function QuaternionFromPoints(V1,V2: TAffineVector): TQuaternion; register; assembler;

// construct a unit quaternion from two points on unit sphere
// EAX contains address of V1
// ECX contains address to result
// EDX contains address of V2

asm
  {Result.Vector[X]:= V1[Y]*V2[Z] - V1[Z]*V2[Y];
  Result.Vector[Y]:= V1[Z]*V2[X] - V1[X]*V2[Z];
  Result.Vector[Z]:= V1[X]*V2[Y] - V1[Y]*V2[X];
  Result.Angle:= V1[X]*V2[X] + V1[Y]*V2[Y] + V1[Z]*V2[Z];}
              FLD DWORD PTR [EDX+8]    // first load both vectors onto FPU register stack
              FLD DWORD PTR [EDX+4]
              FLD DWORD PTR [EDX+0]
              FLD DWORD PTR [EAX+8]
              FLD DWORD PTR [EAX+4]
              FLD DWORD PTR [EAX+0]

              FLD ST(1)                // ST(0):=V1[Y]
              FMUL ST,ST(6)            // ST(0):=V1[Y]*V2[Z]
              FLD ST(3)                // ST(0):=V1[Z]
              FMUL ST,ST(6)            // ST(0):=V1[Z]*V2[Y]
              FSUBP ST(1),ST           // ST(0):=ST(1)-ST(0)
              WAIT
              FSTP DWORD [ECX]         // Result.Vector[X]:=ST(0)
              FLD ST(2)                // ST(0):=V1[Z]
              FMUL ST,ST(4)            // ST(0):=V1[Z]*V2[X]
              FLD ST(1)                // ST(0):=V1[X]
              FMUL ST,ST(7)            // ST(0):=V1[X]*V2[Z]
              FSUBP ST(1),ST           // ST(0):=ST(1)-ST(0)
              WAIT
              FSTP DWORD [ECX+4]       // Result.Vector[Y]:=ST(0)
              FLD ST                   // ST(0):=V1[X]
              FMUL ST,ST(5)            // ST(0):=V1[X]*V2[Y]
              FLD ST(2)                // ST(0):=V1[Y]
              FMUL ST,ST(5)            // ST(0):=V1[Y]*V2[X]
              FSUBP ST(1),ST           // ST(0):=ST(1)-ST(0)
              WAIT
              FSTP DWORD [ECX+8]       // Result.Vector[Z]:=ST(0)
              FMUL ST,ST(3)            // ST(0):=V1[X]*V2[X]
              FLD ST(1)                // ST(0):=V1[Y]
              FMUL ST,ST(5)            // ST(0):=V1[Y]*V2[Y]
              FADDP ST(1),ST           // ST(0):=V1[X]*V2[X]+V1[Y]*V2[Y]
              FLD ST(2)                // etc...
              FMUL ST,ST(6)
              FADDP ST(1),ST
              WAIT
              FSTP DWORD PTR [ECX+12]  // Result.Angle:=ST(0)
              FFREE ST                 // clear FPU register stack
              FFREE ST(1)
              FFREE ST(2)
              FFREE ST(3)
              FFREE ST(4)
end;

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

function QuaternionMultiply(qL, qR: TQuaternion): TQuaternion; register; 

// Return quaternion product qL * qR.  Note: order is important!
// To combine rotations, use the product QuaternionMuliply(qSecond, qFirst),
// which gives the effect of rotating by qFirst then qSecond.

begin
  Result.Angle:=qL.Angle*qR.Angle-qL.Vector[X]*qR.Vector[X]-
                qL.Vector[Y]*qR.Vector[Y]-qL.Vector[Z]*qR.Vector[Z];
  Result.Vector[X]:=qL.Angle*qR.Vector[X]+qL.Vector[X]*qR.Angle+
                    qL.Vector[Y]*qR.Vector[Z]-qL.Vector[Z]*qR.Vector[Y];
  Result.Vector[Y]:=qL.Angle*qR.Vector[Y]+qL.Vector[Y]*qR.Angle+
                    qL.Vector[Z]*qR.Vector[X]-qL.Vector[X]*qR.Vector[Z];
  Result.Vector[Z]:=qL.Angle*qR.Vector[Z]+qL.Vector[Z]*qR.Angle+
                    qL.Vector[X]*qR.Vector[Y]-qL.Vector[Y]*qR.Vector[X];
end;

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

function QuaternionToMatrix(Q: TQuaternion): TMatrix; register;

// Construct rotation matrix from (possibly non-unit) quaternion.
// Assumes matrix is used to multiply column vector on the left:
// vnew = mat vold.  Works correctly for right-handed coordinate system
// and right-handed rotations.

var Norm,S,
    XS,YS,ZS,
    WX,WY,WZ,
    XX,XY,XZ,
    YY,YZ,ZZ   : Single;

begin
  Norm:=Q.Vector[X]*Q.Vector[X]+Q.Vector[Y]*Q.Vector[Y]+Q.Vector[Z]*Q.Vector[Z]+Q.Angle*Q.Angle;
  if Norm > 0 then S:=2/Norm
              else S:=0;
              
  XS:=Q.Vector[X]*S;  YS:=Q.Vector[Y]*S;  ZS:=Q.Vector[Z]*S;
  WX:=Q.Angle*XS;     WY:=Q.Angle*YS;     WZ:=Q.Angle*ZS;
  XX:=Q.Vector[X]*XS; XY:=Q.Vector[X]*YS; XZ:=Q.Vector[X]*ZS;
  YY:=Q.Vector[Y]*YS; YZ:=Q.Vector[Y]*ZS; ZZ:=Q.Vector[Z]*ZS;

  Result[X,X]:=1-(YY+ZZ); Result[Y,X]:=XY+WZ;     Result[Z,X]:=XZ-WY;     Result[W,X]:=0;
  Result[X,Y]:=XY-WZ;     Result[Y,Y]:=1-(XX+ZZ); Result[Z,Y]:=YZ+WX;     Result[W,Y]:=0;
  Result[X,Z]:=XZ+WY;     Result[Y,Z]:=YZ-WX;     Result[Z,Z]:=1-(XX+YY); Result[W,Z]:=0;
  Result[X,W]:=0;         Result[Y,W]:=0;         Result[Z,W]:=0;         Result[W,W]:=1;
end;

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

procedure QuaternionToPoints(Q: TQuaternion; var ArcFrom, ArcTo: TVector); register;

// convert a unit quaternion into two points on a unit sphere

var S: Single;

begin
    S:=Sqrt(Q.Vector[X]*Q.Vector[X]+Q.Vector[Y]*Q.Vector[Y]);
    if s = 0 then ArcFrom:=MakeVector([0,1,0,0])
             else ArcFrom:=MakeVector([-Q.Vector[Y]/S,Q.Vector[X]/S,0,0]);
    ArcTo[X]:=Q.Angle*ArcFrom[X]-Q.Vector[Z]*ArcFrom[Y];
    ArcTo[Y]:=Q.Angle*ArcFrom[Y]+Q.Vector[Z]*ArcFrom[X];
    ArcTo[Z]:=Q.Vector[X]*ArcFrom[Y]-Q.Vector[Y]*ArcFrom[X];
    if Q.Angle < 0 then ArcFrom:=MakeVector([-ArcFrom[X],-ArcFrom[Y],0,0]);
end;

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

function VectorNorm(V: array of Single): Single; assembler; register;

// calculate norm of a vector which is defined as norm=x*x+y*y+...
// EAX contains address of V
// EDX contains highest index in V
// result is passed in ST(0)

asm
              FLDZ                          // initialize sum
@@Loop:       FLD  DWORD PTR [EAX+4*EDX]    // load a component
              FMUL ST,ST                    // make square
              FADDP                         // add previous calculated sum
              SUB  EDX,1
              JNL  @@Loop
end;

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

function VectorNormalize(V: array of Single): Single; assembler; register;

// transform a vector to unit length and return length
// EAX contains address of V
// EDX contains the highest index in V

asm
              PUSH EBX
              MOV ECX,EDX                   // save size of V
              CALL VectorLength             // calculate length of vector
              FTST                          // test if length = 0
              MOV EBX,EAX                   // save parameter address
              FSTSW AX                      // get test result
              TEST AH,C3                    // check the test result
              JNZ @@Finish
              SUB EBX,4                     // simplyfied address calculation
              INC ECX
              FLD1                          // calculate reciprocal of length
              FDIV ST,ST(1)
@@1:          FLD ST                        // double reciprocal
              FMUL DWORD PTR [EBX+4*ECX]    // scale component
              WAIT
              FSTP DWORD PTR [EBX+4*ECX]    // store result
              LOOP @@1
              FSTP ST                       // remove reciprocal from FPU stack
@@Finish:     POP EBX
end;

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

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

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

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

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

procedure VectorScale(V: array of Single; Factor: Single); assembler; register;

// 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(V: array of Single); assembler; register;

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

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

function VectorAdd(V1,V2: TVector): TVector; register;

// 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(V1,V2: TAffineVector): TAffineVector; register;

// 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(V1,V2: TVector): TVector; register;

// 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(V1,V2: TVector): Single; register;

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

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