Return-Path: <harley@umich.edu>
Date: Sat, 22 Sep 2001 12:26:55 -0400
From: "Prof. Harley Flanders" <harley@umich.edu>
Organization: University of  Michigan
Subject: A contribution from William Egge
Content-Type: text/plain; charset=us-ascii
Content-Transfer-Encoding: 7bit
Status: U
X-UIDL: O6y7s9HkIcxdFwE

Dear Colleagues,

William Egge offers his unit, below, for anyone who can use it.

Before that is a short conversation we had on the subject:

-------
 ----- Original Message -----
 From: "Prof. Harley Flanders" <harley@umich.edu>
 To: "Egge, William" <egge@eggcentric.com>
 Sent: Thursday, September 20, 2001 10:46 AM
 Subject: Math unit

 Dear Billy,

 Thanks -- I'll look it over.  Did you want it distributed
 to the Delphi Scientific mini-group?

 One question: why do you use type Single?  Memory is so cheap and
 plentiful these days, why quibble over a few bytes at the
 cost of accuracy?

 Harley
----------------

Billy wrote:

Its for performance reasons.  Singles run way faster than extended (my
testing a
few years ago).  The math unit is was written for use in a laser
lightshow
controler and the accuracy of a single was good.  Also you will note
there is a
sin cos table and Rotations are based on integer values from 0 to 360...
again, for the laser controler there was no reason to have 0.5 degrees
since it
would look pretty much the same as 1 degree to the user.  In all, its
for high
speed with sufficient accuracy for the laser system or really any on
screen
display.

You could easily modify it to use the more accurate types...

Some good functions:

function DistPoint2Line(const Point, LineP1, LineP2: TZPoint): single;
function DistPoint2LineSegment(const Point, LineStart, LineEnd:
TZPoint): single;
function TriangleArea(const a, b, base: Single): Single;
function TriangleHeight(const a, b, base: Single): Single;

A good one is the TriangleHeight function; ever need to calculate the
hieght of a triangle only given the lengths of the sides?

---
William Egge
http://www.eggcentric.com


===================================================================
unit EMath;

interface
{$ifndef ver80}
uses
  Math;
{$endif}

const
  Pi90 = Pi/2;
  Pi45 = Pi/4;

type
  PD2Point = ^TD2Point;
  TD2Point = record
    X, Y: Single;
  end;

  PZPoint = ^TZPoint;
  TZPoint = record
    X, Y, Z: Single;
  end;
  TVector = TZPoint;

{Core Math routines}
function D2Point(const X, Y: Single): TD2Point;
function ZPoint(const X, Y, Z: single): TZPoint;
function Vector(const X, Y, Z: single): TVector;
function AddVectors(const A, B: TVector): TVector;
function SubtractVectors(const A, MinusB: TVector): TVector;
function Magnitude(const V: TVector): Single;
function DirectionVector(const P1, P2: TZPoint): TVector;
{$ifdef ver80}
function UnitVector_P(const P1, P2: TZPoint): TVector;
function UnitVector_V(const V: TVector): TVector;
{$else}
function UnitVector(const P1, P2: TZPoint): TVector; overload;
function UnitVector(const V: TVector): TVector; overload;
{$endif}
function Cross(const U, V: TVector): TVector;
function Dot(const U, V: TVector): Single;
function MultiplyByScalar(const C: Single; const V: TVector): TVector;

{ Higher level functions }
function Distance(const P1, P2: TZPoint): Single;
function DistPoint2Line(const Point, LineP1, LineP2: TZPoint): single;
function DistPoint2LineSegment(const Point, LineStart, LineEnd:
TZPoint): single;
function AngleD2(X, Y: Single): Single;
function MinAngleBetweenVectors(const P, Q, R: TZPoint): Single;
function MidPoint(const P1, P2: TZPoint): TZPoint;
function TriangleArea(const a, b, base: Single): Single;
function TriangleHeight(const a, b, base: Single): Single;
function PointOnLine(const P1, P2: TZPoint; const t: Single): TZPoint;
function RotatePoint(const P: TZPoint; const APhi, ATheta, ATwist:
Integer): TZPoint;

{Utility function}
function FixIntDegrees(const Angle: Integer): Integer;
function FixIntDegreesAllowNegative(const Angle: Integer): Integer;

{Graphics}
// Make AbsoluteEyeDist the max width of your view area.
function D3ToD2(const ZPoint: TZPoint; const AbsoluteEyeDist: Single =
1024): TD2Point;
function D2ToD3(P: TD2Point; ZPlane: Single; AbsoluteEyeDist: Single =
1024): TZPoint;

{Do not alter these !, values are assigned in initialization section}
var
  { 360 is included to make programming easier }
  SinTable: array[0..360] of single;
  CosTable: array[0..360] of single;

implementation

function D3ToD2(const ZPoint: TZPoint; const AbsoluteEyeDist: Single =
1024): TD2Point;
const
  SZero: Single = 0;
  SOne: Single = 1;
  SNegOne: Single = -1;
var
  D: single;
begin
  // Calculate distance from eye
  D:= (AbsoluteEyeDist+ZPoint.Z);

  if D < SOne then
  begin
    if D > SZero then
      D:= AbsoluteEyeDist+SOne
    else if D > SNegOne then
      D:= AbsoluteEyeDist-SOne
    else if D = SZero then
      D:= SOne;
  end;

  // Do conversion (meaning of D is lost but oh well)
  D:= AbsoluteEyeDist/D;
  Result.Y:= ZPoint.Y*D;
  Result.X:= ZPoint.X*D;
end;



function D2ToD3(P: TD2Point; ZPlane: Single; AbsoluteEyeDist: Single =
1024): TZPoint;
var
  D: Single;
begin
  D:= AbsoluteEyeDist+ZPlane;
  Result.X:= (P.X*D)/AbsoluteEyeDist;
  Result.Y:= (P.Y*D)/AbsoluteEyeDist;
  Result.Z:= ZPlane;
end;

function RotatePoint(const P: TZPoint; const APhi, ATheta, ATwist:
Integer): TZPoint;
var
  TempX, TempY, TempZ: Single;
begin
  Result:= P;
  with Result do
  begin
    {Twist, Clockwise ZSpin}
    if (ATwist>0) then
    begin
      TempX:= CosTable[ATwist]*X - SinTable[ATwist]*Y;
      TempY:= SinTable[ATwist]*X + CosTable[ATwist]*Y;
      X:= TempX;
      Y:= TempY;
    end;

    {Phi, top gets closer first (XRot)}
    if (APhi>0) then
    begin
      TempZ:= CosTable[APhi]*Z - SinTable[APhi]*Y;
      TempY:= SinTable[APhi]*Z + CosTable[APhi]*Y;
      Z:= TempZ;
      Y:= TempY;
    end;

    {Theta, right gets closer first}
    if (ATheta>0) then
    begin
      TempX:= CosTable[ATheta]*X - SinTable[ATheta]*Z;
      TempZ:= SinTable[ATheta]*X + CosTable[ATheta]*Z;
      X:= TempX;
      Z:= TempZ;
    end;
  end;
end;

function FixIntDegrees(const Angle: Integer): Integer;
var
  MakeZeroTo360: Boolean;
begin
  MakeZeroTo360:= Angle <> 0;
  Result:= Angle mod 360;
  if Result < 0 then
    Inc(Result, 360);

  if (Result = 0) and MakeZeroTo360 then
    Result:= 360
end;

function FixIntDegreesAllowNegative(const Angle: Integer): Integer;
var
  MakeZeroTo360: Boolean;
begin
  MakeZeroTo360:= Angle <> 0;
  Result:= Angle mod 360;
  if (Result = 0) and MakeZeroTo360 then
  begin
    if Angle > 0 then
      Result:= 360
    else
      Result:= -360;
  end;
end;

function AngleD2(X, Y: Single): Single;
var
  Q: Integer;
begin
  if X = 0 then
  begin
    if Y > 0 then
      Result:= Pi / 2
    else
    begin
      if Y = 0 then
        Result:= 0
      else
        Result:= 3 * Pi / 2;
    end;
    Exit;
  end;
  if Y = 0 then
  begin
    if X > 0 then
      Result:= 0
    else
      Result:= Pi;
    Exit;
  end;

  if (X>0) then
  begin
    if Y>=0 then
      Q:= 1
    else
      Q:= 4;
  end
  else
  begin
    if Y<0 then
      Q:= 3
    else
      Q:= 2;
  end;
  try
    Result:= ArcTan(abs(Y/X));
  except
    Result:= Pi / 2;
  end;
  case Q of
    2: Result:= (Pi) - Result;
    3: Result:= Result+Pi;
    4: Result:= 2*Pi - Result;
  end;
end;

function D2Point(const X, Y: Single): TD2Point;
begin
  Result.X:= X;
  Result.Y:= Y;
end;

function ZPoint(const X, Y, Z: single): TZPoint;
begin
  Result.X:= X;
  Result.Y:= Y;
  Result.Z:= Z;
end;

function Vector(const X, Y, Z: single): TVector;
begin
  Result.X:= X;
  Result.Y:= Y;
  Result.Z:= Z;
end;

function AddVectors(const A, B: TVector): TVector;
begin
  with Result do
  begin
    X:= A.X+B.X;
    Y:= A.Y+B.Y;
    Z:= A.Z+B.Z;
  end;
end;

function SubtractVectors(const A, MinusB: TVector): TVector;
begin
  with Result do
  begin
    X:= A.X-MinusB.X;
    Y:= A.Y-MinusB.Y;
    Z:= A.Z-MinusB.Z;
  end;
end;

function MultiplyByScalar(const C: Single; const V: TVector): TVector;
begin
  Result.X:= V.X*C;
  Result.Y:= V.Y*C;
  Result.Z:= V.Z*C;
end;

function Magnitude(const V: TVector): Single;
begin
  Result:= Sqrt((V.X*V.X) + (V.Y*V.Y) + (V.Z*V.Z));
end;

function DirectionVector(const P1, P2: TZPoint): TVector;
begin
  Result:= SubtractVectors(P2, P1);
end;

{$ifdef ver80}
function UnitVector_P(const P1, P2: TZPoint): TVector;
begin
  Result:= UnitVector_V(DirectionVector(P1, P2));
end;
{$else}
function UnitVector(const P1, P2: TZPoint): TVector;
begin
  Result:= UnitVector(DirectionVector(P1, P2));
end;
{$endif}

{$ifdef ver80}
function UnitVector_V(const V: TVector): TVector;
{$else}
function UnitVector(const V: TVector): TVector;
{$endif}
var
  M: Single;
begin
  Result:= V;
  M:= Magnitude(Result);
  if M <> 0 then
    with Result do
    begin
      X:= X/M;
      Y:= Y/M;
      Z:= Z/M;
    end;
end;

function MinAngleBetweenVectors(const P, Q, R: TZPoint): Single;
var
  CosAngle: Single;
  V1, V2: TVector;
  Mag1, Mag2: Single;
  MagMult: Single;
begin
  V1:= DirectionVector(P, Q);
  V2:= DirectionVector(P, R);
  Mag1:= Magnitude(V1);
  Mag2:= Magnitude(V2);
  MagMult:= Mag1*Mag2;
  if (MagMult=0) then
    Result:= 0
  else
  begin
    CosAngle:= Dot(V1, V2)/MagMult;
    {Round errors}
    if CosAngle > 1 then
      CosAngle:= 1
    else if CosAngle < -1 then
      CosAngle:= -1;
{$ifdef ver80}
    Result:= ArcTan(sqrt(1-sqr(CosAngle)) / CosAngle);
{$else}
    Result:= ArcCos(CosAngle);
{$endif}
  end;
end;

function Cross(const U, V: TVector): TVector;
begin
  Result:= Vector(
    (U.Y*V.Z)-(U.Z*V.Y),
    -((U.X*V.Z)-(U.Z*V.X)),
    (U.X*V.Y)-(U.Y*V.X)
  );
end;

function Dot(const U, V: TVector): Single;
begin
  Result:= (U.X*V.X) + (U.Y*V.Y) + (U.Z*V.Z);
end;

function DistPoint2Line(const Point, LineP1, LineP2: TZPoint): single;
var
  U: TVector;
  MU: Single;
  P: TZPoint;
  PQ: TVector;
  PQCrossU: TVector;
begin
  {Bottom of Page 715 in Swokowski Calculus 5th Edition}
  U:= DirectionVector(LineP1, LineP2);
  MU:= Magnitude(U);
  if (MU<0.0000000001) then
  begin
    Result:= Magnitude(DirectionVector(Point, LineP1));
  end
  else
  begin
    P:= LineP1;
    PQ:= DirectionVector(P, Point);
    PQCrossU:= Cross(PQ, U);
    try
      Result:= Magnitude(PQCrossU)/MU;
    except
      Result:= Magnitude(DirectionVector(Point, LineP1));
    end;
  end;
end;

function DistPoint2LineSegment(const Point, LineStart, LineEnd:
TZPoint): single;
{
  Concept
  If the angle between either endpoint and the dot is greater than 90
  then the dot is outside of the line segment. I use a test and use
  the dot product because if the cos is negative then the angle is
greater
  than 90. The dot product will be negative if the angle is greater than
90
  See page 703 in Swokowski Calculus 5th Edition for dot product cos
reason
}
begin
  if Dot(DirectionVector(LineStart, Point), DirectionVector(LineStart,
LineEnd)) < 0 then
    Result:= Magnitude(DirectionVector(LineStart, Point))
  else if Dot(DirectionVector(LineEnd, Point), DirectionVector(LineEnd,
LineStart)) < 0 then
    Result:= Magnitude(DirectionVector(LineEnd, Point))
  else
    Result:= DistPoint2Line(Point, LineStart, LineEnd);
end;

function MidPoint(const P1, P2: TZPoint): TZPoint;
const
  CHalf: Single = 0.5;
begin
  Result.X:= (P1.X+P2.X)*CHalf;
  Result.Y:= (P1.Y+P2.Y)*CHalf;
  Result.Z:= (P1.Z+P2.Z)*CHalf;
end;

function MidPointx(const P1, P2: TZPoint): TZPoint;
const
  CHalf: Single = 0.5;
asm
  { eax -@P1
    edx -@P2
    ecx -@Result

    Memory bandwidth is the main performance problem in this function. }

  { load inverse of two onto stack }
  fld CHalf

  { load P1.X, P1.Y & P1.Z }
  fld [eax].TZPoint.X  // P1.X | 0.5
  fld [eax].TZPoint.Y  // P1.Y | P1.X | 0.5
  fld [eax].TZPoint.Z  // P1.Z | P1.Y | P1.X | 0.5

  { load P2.X, P2.Y, P2.Z and calculate (P2.?+P1.?) for each }
  fld [edx].TZPoint.X   // P2.X | P1.Z | P1.Y | P1.X | 0.5
  faddp st(3), st(0)    // P1.Z | P1.Y | P2.X+P1.X  | 0.5
  fld [edx].TZPoint.Y   // P2.Y | P1.Z | P1.Y | P2.X+P1.X  | 0.5
  faddp st(2), st(0)    // P1.Z | P2+P1.Y | P2.X+P1.X  | 0.5
  fld [edx].TZPoint.Z   // P2.Z | P1.Z | P2+P1.Y | P2.X+P1.X  | 0.5
  faddp st(1), st(0)    // P2.Z+P1.Z | P2+P1.Y | P2.X+P1.X  | 0.5

  { next, perform *0.5 for each one (and call this A,B,C for brevity) }
  fmul st(0), st(3)  //     C      | P2+P1.Y | P2.X+P1.X  | 0.5
  fxch st(2)         // P2.X+P1.X  | P2+P1.Y |     C      | 0.5
  fmul st(0), st(3)  //     A      | P2+P1.Y |     C      | 0.5
  fxch st(3)         //    0.5     | P2+P1.Y |     C      |  A
  fmulp st(1), st(0) //     B      |    C    |     A


  { store stuff to memory }
  fstp [ecx].TZPoint.Y
  fstp [ecx].TZPoint.Z
  fstp [ecx].TZPoint.X

  { raise exceptions if any caused }
  fwait
end;

function MidPointy(const P1, P2: TZPoint): TZPoint;
const
  CHalf: Single = 0.5;
asm
  { eax -@P1
    edx -@P2
    ecx -@Result

    Memory bandwidth is the main performance problem in this function. }

  { load inverse of two onto stack }
  fld CHalf

  { load P1.X, P1.Y & P1.Z }
  fld [eax].TZPoint.X  // P1.X | 0.5
  fld [eax].TZPoint.Y  // P1.Y | P1.X | 0.5
  fld [eax].TZPoint.Z  // P1.Z | P1.Y | P1.X | 0.5

  { load P2.X, P2.Y, P2.Z and calculate (P2.?-P1.?) for each }
  fld [edx].TZPoint.X  // P2.X | P1.Z | P1.Y | P1.X | 0.5
  fsub st(0), st(3)    // P2.X-P1.X | P1.Z | P1.Y | P1.X | 0.5
  fld [edx].TZPoint.Y  // P2.Y | P2.X-P1.X | P1.Z | P1.Y | P1.X | 0.5
  fsub st(0), st(3)    // P2.Y-P1.Y | P2.X-P1.X | P1.Z | P1.Y | P1.X |
0.5
  fld [edx].TZPoint.Z  // P2.Z | P2.Y-P1.Y | P2.X-P1.X | P1.Z | P1.Y |
P1.X | 0.5
  fsub st(0), st(3)    // P2.Z-P1.Z | P2.Y-P1.Y | P2.X-P1.X | P1.Z |
P1.Y | P1.X | 0.5

  { next, perform *0.5 for each one (and call this A,B,C for brevity) }
  fxch st(2)         // P2.X-P1.X | P2.Y-P1.Y | P2.Z-P1.Z | P1.Z | P1.Y
| P1.X | 0.5
  fmul st(0), st(6)  // A | P2.Y-P1.Y | P2.Z-P1.Z | P1.Z | P1.Y | P1.X |
0.5
  fxch st(1)         // P2.Y-P1.Y | A | P2.Z-P1.Z | P1.Z | P1.Y | P1.X |
0.5
  fmul st(0), st(6)  // B | A | P2.Z-P1.Z | P1.Z | P1.Y | P1.X | 0.5
  { don't need 0.5 after this, get rid of it }
  fxch st(6)         // 0.5 | A | P2.Z-P1.Z | P1.Z | P1.Y | P1.X | B
  fmulp st(2), st(0) // A | C | P1.Z | P1.Y | P1.X | B

  { next, perform +P1.? to get Result.? }
  faddp st(4), st(0) // C | P1.Z | P1.Y | Result.X | B
  fxch st(4)         // B | P1.Z | P1.Y | Result.X | C
  faddp st(2), st(0) // P1.Z | Result.Y | Result.X | C
  faddp st(3), st(0) // Result.Y | Result.X | Result.Z

  { store stuff to memory }
  fxch st(1)  // Result.X | Result.Y | Result.Z
  fstp [ecx].TZPoint.X
  fstp [ecx].TZPoint.Y
  fstp [ecx].TZPoint.Z

  { raise exceptions if any caused }
  fwait
end;

function TriangleArea(const a, b, base: Single): Single;
var
  s: Single;
begin
  s:= (a+b+base)*0.5;
  Result:= Sqrt(s*(s - a)*(s - b)*(s - base));
end;

function TriangleHeight(const a, b, base: Single): Single;
begin
  Result:= (TriangleArea(a, b, base)*2)/base;
end;

function Distance(const P1, P2: TZPoint): Single;
begin
  Result:= Magnitude(DirectionVector(P1, P2));
end;

function PointOnLine(const P1, P2: TZPoint; const t: Single): TZPoint;
begin
  Result:= DirectionVector(P1, P2);
  Result:= MultiplyByScalar(t, Result);
  Result:= AddVectors(P1, Result);
end;

procedure InitCosSinTables;
var
  I: Integer;
begin
  for I:= 0 to 360 do
  begin
    SinTable[I]:= sin((I/180)*Pi);
    CosTable[I]:= cos((I/180)*Pi);
  end;
end;

initialization
  InitCosSinTables;

end.


--
=====================================================================
Prof. Harley Flanders