G3dVectorImpl.mesa
Copyright Ó 1984, 1992 by Xerox Corporation. All rights reserved.
Bloomenthal, April 9, 1993 4:11 pm PDT
Heckbert, June 23, 1988 1:49:10 am PDT
Glassner, February 14, 1991 9:02 am PST
DIRECTORY G2dBasic, G3dBasic, G3dMatrix, G3dVector, Real, RealFns;
Types
Box: TYPE ~ G3dBasic.Box;
Pair: TYPE ~ G3dBasic.Pair;
Triple: TYPE ~ G3dBasic.Triple;
Quad: TYPE ~ G3dBasic.Quad;
Ray: TYPE ~ G3dBasic.Ray;
TripleSequence: TYPE ~ G3dBasic.TripleSequence;
Matrix: TYPE ~ G3dMatrix.Matrix;
NearSegment:
TYPE ~ G3dVector.NearSegment;
origin: Triple ~ G3dBasic.origin;
Basic Operations on a Single Vector
Null:
PUBLIC
PROC [v: Triple]
RETURNS [
BOOL] ~ {
RETURN[v.x = 0.0 AND v.y = 0.0 AND v.z = 0.0];
};
Negate:
PUBLIC PROC [v: Triple]
RETURNS [Triple] ~ {
RETURN[[-v.x, -v.y, -v.z]];
};
Unit:
PUBLIC
PROC [v: Triple]
RETURNS [ret: Triple] ~ {
m: REAL ¬ v.x*v.x+v.y*v.y+v.z*v.z;
SELECT m
FROM
0.0, 1.0 => RETURN[v];
ENDCASE => {
m ¬ RealFns.SqRt[m];
RETURN[[v.x/m, v.y/m, v.z/m]];
};
};
Mul:
PUBLIC PROC [v: Triple, s:
REAL]
RETURNS [Triple] ~ {
RETURN[[v.x*s, v.y*s, v.z*s]];
};
Div:
PUBLIC PROC [v: Triple, s:
REAL]
RETURNS [Triple] ~ {
RETURN[IF s # 0.0 THEN [v.x/s, v.y/s, v.z/s] ELSE [v.x, v.y, v.z]];
};
Abs:
PUBLIC PROC [v: Triple]
RETURNS [Triple] ~ {
RETURN[[ABS[v.x], ABS[v.y], ABS[v.z]]];
};
Basic Operations on Two Vectors
Add:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN[[v2.x+v1.x, v2.y+v1.y, v2.z+v1.z]];
};
Sub:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN[[v1.x-v2.x, v1.y-v2.y, v1.z-v2.z]];
};
Equal:
PUBLIC
PROC [v1, v2: Triple, epsilon:
REAL ¬ 0.001]
RETURNS [
BOOL] ~ {
RETURN[ABS[v1.x-v2.x]<epsilon AND ABS[v1.y-v2.y]<epsilon AND ABS[v1.z-v2.z] < epsilon];
};
Dot:
PUBLIC PROC [v1, v2: Triple]
RETURNS [
REAL] ~ {
RETURN[v1.x*v2.x + v1.y*v2.y + v1.z*v2.z];
};
Cross:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN[[v1.y*v2.z-v1.z*v2.y, v1.z*v2.x-v1.x*v2.z, v1.x*v2.y-v1.y*v2.x]];
};
Midpoint:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN[[0.5*(v1.x+v2.x), 0.5*(v1.y+v2.y), 0.5*(v1.z+v2.z)]];
};
Interp:
PUBLIC PROC [t:
REAL, v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN [[v1.x+t*(v2.x-v1.x), v1.y+t*(v2.y-v1.y), v1.z+t*(v2.z-v1.z)]];
};
InterpQuad:
PUBLIC PROC [t:
REAL, v1, v2: Quad]
RETURNS [Quad] ~ {
RETURN[[v1.x+t*(v2.x-v1.x), v1.y+t*(v2.y-v1.y), v1.z+t*(v2.z-v1.z), v1.w+t*(v2.w-v1.w)]];
};
Combine:
PUBLIC
PROC [v1: Triple, s1:
REAL, v2: Triple, s2:
REAL]
RETURNS [Triple] ~ {
RETURN[[s1*v1.x+s2*v2.x, s1*v1.y+s2*v2.y, s1*v1.z+s2*v2.z]];
};
ScaleRay:
PUBLIC
PROC [ray: Ray, d:
REAL]
RETURNS [Triple] ~ {
RETURN[[ray.base.x+d*ray.axis.x, ray.base.y+d*ray.axis.y, ray.base.z+d*ray.axis.z]];
};
MulVectors:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN[[v1.x*v2.x, v1.y*v2.y, v1.z*v2.z]];
};
DivVectors:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
ret: Triple ¬ v1;
IF v2.x # 0.0 THEN ret.x ¬ ret.x/v2.x;
IF v2.y # 0.0 THEN ret.y ¬ ret.y/v2.y;
IF v2.z # 0.0 THEN ret.z ¬ ret.z/v2.z;
RETURN[ret];
};
Basic Operations on Sequence of Vectors
UnitizeSequence:
PUBLIC
PROC [triples: TripleSequence] ~ {
IF triples #
NIL
THEN
FOR n:
NAT
IN [0..triples.length)
DO
triples[n] ¬ Unit[triples[n]];
ENDLOOP;
};
NegateSequence:
PUBLIC
PROC [triples: TripleSequence] ~ {
IF triples #
NIL
THEN
FOR n:
NAT
IN [0..triples.length)
DO
triples[n] ¬ Negate[triples[n]];
ENDLOOP;
};
MinMaxSequence:
PUBLIC PROC [triples: TripleSequence]
RETURNS [b: Box] ~ {
huge: REAL ~ Real.LargestNumber;
b.min ¬ [huge, huge, huge];
b.max ¬ [-huge, -huge, -huge];
IF triples = NIL THEN RETURN[[origin, origin]];
FOR n:
NAT
IN [0..triples.length)
DO
t: Triple ~ triples[n];
IF t.x < b.min.x THEN b.min.x ¬ t.x;
IF t.x > b.max.x THEN b.max.x ¬ t.x;
IF t.y < b.min.y THEN b.min.y ¬ t.y;
IF t.y > b.max.y THEN b.max.y ¬ t.y;
IF t.z < b.min.z THEN b.min.z ¬ t.z;
IF t.z > b.max.z THEN b.max.z ¬ t.z;
ENDLOOP;
};
Length and Distance Operations
Length:
PUBLIC PROC [v: Triple]
RETURNS [
REAL] ~ {
RETURN[RealFns.SqRt[v.x*v.x+v.y*v.y+v.z*v.z]];
};
Square:
PUBLIC PROC [v: Triple]
RETURNS [
REAL] ~ {
RETURN[v.x*v.x+v.y*v.y+v.z*v.z];
};
Distance:
PUBLIC PROC [p1, p2: Triple]
RETURNS [
REAL] ~ {
a: REAL ¬ p2.x-p1.x;
b: REAL ¬ p2.y-p1.y;
c: REAL ¬ p2.z-p1.z;
RETURN[RealFns.SqRt[a*a+b*b+c*c]];
};
SquareDistance:
PUBLIC PROC [p1, p2: Triple]
RETURNS [
REAL] ~ {
RETURN[Square[Sub[p1, p2]]];
};
SameLength:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
lengthV2: REAL ¬ Length[v2];
RETURN[IF lengthV2 # 0.0 THEN Mul[v2, Length[v1]/lengthV2] ELSE v2];
};
SetVectorLength:
PUBLIC PROC [v: Triple, length:
REAL]
RETURNS [Triple] ~ {
sqLength: REAL ¬ v.x*v.x+v.y*v.y+v.z*v.z;
IF sqLength = 0.0 THEN RETURN[v];
IF sqLength # 1.0 THEN length ¬ length/RealFns.SqRt[sqLength];
RETURN[[v.x*length, v.y*length, v.z*length]];
};
Directional Tests
Collinear:
PUBLIC
PROC [p1, p2, p3: Triple, epsilon:
REAL ¬ 0.01]
RETURNS [
BOOL] ~ {
RETURN[Parallel[Sub[p1, p2], Sub[p2, p3], epsilon]];
};
VecsCoplanar:
PUBLIC PROC [v1, v2, v3: Triple, epsilon:
REAL ¬ 0.01]
RETURNS [
BOOL] ~ {
Compute determinant of matrix of vectors:
e1: REAL ¬ v1.x*(v2.y*v3.z-v3.y*v2.z);
e2: REAL ¬ v1.y*(v2.x*v3.z-v3.x*v2.z);
e3: REAL ¬ v1.z*(v2.x*v3.y-v3.x*v2.y);
RETURN[ABS[e2-e1-e3] < epsilon];
};
PointsCoplanar:
PUBLIC PROC [p1, p2, p3, p4: Triple, epsilon:
REAL ¬ 0.01]
RETURNS [BOOL]
~ {
RETURN[VecsCoplanar[Sub[p4, p1], Sub[p3, p1], Sub[p2, p1], epsilon]];
};
Parallel:
PUBLIC
PROC [v1, v2: Triple, epsilon:
REAL ¬ 0.005]
RETURNS [
BOOL] ~ {
RETURN[ABS[CosineBetween[v1, v2, TRUE]] > 1.-epsilon];
The following can be completely bogus (in particular, it can ignore epsilon):
ratio: REAL ← Real.LargestNumber;
IF v1 = v2 THEN RETURN[TRUE];
IF (v1.x=0) # (v2.x=0) OR (v1.y=0) # (v2.y=0) OR (v1.z=0) # (v2.z=0)
THEN RETURN[FALSE];
IF v1.x # 0.0 THEN ratio ← v2.x/v1.x;
IF v1.y # 0.0 THEN {
IF ratio = Real.LargestNumber
THEN ratio ← v2.y/v1.y
ELSE IF ABS[ratio-(v2.y/v1.y)] > epsilon THEN RETURN[FALSE];
};
IF ratio = Real.LargestNumber OR v1.z = 0.0 THEN RETURN[TRUE];
RETURN[ABS[ratio-(v2.z/v1.z)] <= epsilon];
};
Perpendicular:
PUBLIC
PROC [v1, v2: Triple, epsilon:
REAL ¬ 0.005]
RETURNS [
BOOL] ~ {
RETURN[ABS[CosineBetween[v1, v2]] < epsilon];
};
FrontFacing:
PUBLIC
PROC [vector, base: Triple, view: Matrix]
RETURNS [
BOOL] ~ {
IF G3dMatrix.HasPerspective[view]
THEN RETURN[FrontFacingWithPerspective[vector, base, G3dMatrix.Invert[view]]]
ELSE RETURN[FrontFacingNoPerspective[vector, view]];
};
FrontFacingNoPerspective:
PUBLIC PROC [vector: Triple, view: Matrix]
RETURNS [
BOOL] ~ {
RETURN[vector.x*view[0][2]+vector.y*view[1][2]+vector.z*view[2][2] < 0.0];
};
FrontFacingWithPerspective:
PUBLIC PROC [vector, base: Triple, invView: Matrix]
RETURNS [BOOL] -- New, improved! Faster!! This should always work.
This presumes invView to be the INVERSE of the actual world-to-view matrix.
~ {
same as: camera: Triple ¬ G3dMatrix.Transform[[0, 0, 0], invView]:
q: Quad ¬ [invView[3][0], invView[3][1], invView[3][2], invView[3][3]];
camera: Triple ¬ IF q.w = 1.0 THEN [q.x, q.y, q.z] ELSE [q.x/q.w, q.y/q.w, q.z/q.w];
RETURN[Dot[Sub[camera, base], vector] > 0.0];
the following way is fine, and equivalent to the above, but takes more multiplies:
plane: Plane ¬ G3dPlane.FromPointAndNormal[base, vector];
RETURN[
invView[2][0]*plane.x+
invView[2][1]*plane.y+
invView[2][2]*plane.z+
invView[2][3]*plane.w < 0.0];
};
FrontFacingWithPerspective: PUBLIC PROC [vector, base: Triple, view: Matrix]
RETURNS [BOOL] -- Way it was yesterday . . . (this is faulty)
~ {
end: Triple ~ Add[base, vector];
z0: REAL ← base.x*view[0][2]+base.y*view[1][2]+base.z*view[2][2]+view[3][2];
z1: REAL ← end.x*view[0][2]+end.y*view[1][2]+end.z*view[2][2]+view[3][2];
RETURN[z1 < z0];
};
FrontFacingWithPerspective: PUBLIC PROC [vector, base: Triple, view: Matrix]
RETURNS [BOOL] -- Way it was a few minutes ago . . .
(this probably corrects above as long as vector doesn't go behind the eye!)
~ {
end: Triple ~ Add[base, vector];
z0: REAL ← base.x*view[0][2]+base.y*view[1][2]+base.z*view[2][2]+view[3][2];
w0: REAL ← base.x*view[0][3]+base.y*view[1][3]+base.z*view[2][3]+view[3][3];
z1: REAL ← end.x*view[0][2]+end.y*view[1][2]+end.z*view[2][2]+view[3][2];
w1: REAL ← end.x*view[0][3]+end.y*view[1][3]+end.z*view[2][3]+view[3][3];
RETURN[z1/w1 < z0/w0];
};
Nearness Operations
NearnessAccelerator:
PUBLIC
PROC [p0, p1: Triple]
RETURNS [n: Quad] ~ {
n.x ¬ p1.x-p0.x;
n.y ¬ p1.y-p0.y;
n.z ¬ p1.z-p0.z;
n.w ¬ n.x*n.x+n.y*n.y+n.z*n.z;
IF n.w = 0.0 THEN RETURN;
n.x ¬ n.x/n.w;
n.y ¬ n.y/n.w;
n.z ¬ n.z/n.w;
};
NearestToSegment:
PUBLIC
PROC [p0, p1, q: Triple, acc: Quad ¬ [0.0, 0.0, 0.0, 0.0]]
RETURNS [nearest: NearSegment]
~ {
IF acc # [0.0, 0.0, 0.0, 0.0]
THEN {
alpha: REAL ¬ (q.x-p0.x)*acc.x+(q.y-p0.y)*acc.y+(q.z-p0.z)*acc.z;
SELECT alpha
FROM
<= 0.0 => {
nearest.inside ¬ FALSE;
nearest.point ¬ p0;
nearest.w0 ¬ 1.0;
nearest.w1 ¬ 0.0;
};
>= 1.0 => {
nearest.inside ¬ FALSE;
nearest.point ¬ p1;
nearest.w0 ¬ 0.0;
nearest.w1 ¬ 1.0;
};
ENDCASE => {
nearest.inside ¬ TRUE;
nearest.w1 ¬ alpha;
nearest.w0 ¬ 1.0-nearest.w1;
alpha ¬ alpha*acc.w;
nearest.point ¬ [p0.x+alpha*acc.x, p0.y+alpha*acc.y, p0.z+alpha*acc.z];
};
}
ELSE {
delta: Triple ¬ [p1.x-p0.x, p1.y-p0.y, p1.z-p0.z];
nearest.point ¬ p0;
IF delta = origin
THEN RETURN
ELSE {
ua: Triple ~ [q.x-p0.x, q.y-p0.y, q.z-p0.z];
deltaSq: REAL ~ delta.x*delta.x+delta.y*delta.y+delta.z*delta.z;
alpha: REAL ¬ (ua.x*delta.x+ua.y*delta.y+ua.z*delta.z)/deltaSq;
SELECT alpha
FROM
< 0.0 => {nearest.inside ¬ FALSE; alpha ¬ 0.0};
> 1.0 => {nearest.inside ¬ FALSE; alpha ¬ 1.0};
ENDCASE => nearest.inside ¬ TRUE;
nearest.point ¬ [p0.x+alpha*delta.x, p0.y+alpha*delta.y, p0.z+alpha*delta.z];
nearest.w1 ¬ alpha;
nearest.w0 ¬ 1.0-alpha;
};
};
};
NearestToLine:
PUBLIC PROC [line: Ray, q: Triple]
RETURNS [Triple] ~ {
[Artwork node; type 'Artwork on' to command tool]
u: Triple ¬ Unit[line.axis];
RETURN[Add[line.base, Mul[u, Dot[Sub[q, line.base], u]]]];
};
PointOnLine:
PUBLIC
PROC [p: Triple, line: Ray, epsilon:
REAL ¬ 0.005]
RETURNS [
BOOL] ~ {
RETURN[Distance[p, NearestToLine[line, p]] < epsilon];
};
NearestToSequence:
PUBLIC
PROC
[p:
Triple,
points:
TripleSequence]
RETURNS
[index:
NAT ¬ 0]
~ {
minSqDist: REAL ¬ Real.LargestNumber;
IF points #
NIL THEN
FOR n:
NAT
IN [0..points.length)
DO
sqDist: REAL ¬ SquareDistance[p, points[n]];
IF sqDist < minSqDist THEN {index ¬ n; minSqDist ¬ sqDist};
ENDLOOP;
};
ClosestApproach2Lines:
PUBLIC PROC [line1, line2: Ray]
RETURNS [p1, p2: Triple] ~ {
Presumes p1 and p2 form a line segment that is perpendicular to each of the lines.
p1 and p2 are given as line1.base+s(line1.axis) and line2.base+t(line2.axis);
(p2-p1).line1.axis = 0 and (p2-p1).line2.axis = 0.
axis1: Triple ¬ Unit[line1.axis];
axis2: Triple ¬ Unit[line2.axis];
den: REAL ¬ Dot[axis1, axis2];
densq: REAL ¬ den*den;
dif: Triple ¬ Sub[line1.base, line2.base];
dot1: REAL ¬ Dot[axis1, dif];
dot2: REAL ¬ Dot[axis2, dif];
s: REAL ¬ (dot1/densq-dot2/den)/(1.0-1.0/densq);
t: REAL ¬ (s+dot1)/den;
p1 ¬ Add[line1.base, Mul[axis1, s]];
p2 ¬ Add[line2.base, Mul[axis2, t]];
};
DistanceBetween2Lines:
PUBLIC PROC [line1, line2: Ray]
RETURNS [
REAL] ~ {
From Bowyer and Woodwark (A Programmer's Geometry)
P. Heckbert writes
It looks like the Bowyer and Woodwark formula is equivalent to
abs((P2-P1).D1xD2) / ||D1 D2||
where A.B C is a triple scalar product, equivalent to the determinant of the 3x3 matrix
with 3-vectors A, B, and C as rows. Their variables ab, bc, ca are components of vector
D1xD2. Since ||D1xD2|| = ||D1|| * ||D2|| * |sin(theta)|, and A.BxC is the volume of the
parallelepiped with sides A, B, and C, imagine this parallelepiped with one edge parallel to
and sliding along line 1, one edge sliding along line 2, and the other edge connecting one
point of line 1 (P1) to one point of line 2 (P2). If you kept the edge on line 1 fixed and
changed P2, the origin point of line 2, stretching the parallelogram like a block of jello,
the volume of the block would stay fixed because its base has constant area of ||D1xD2||
and its altitude is constant (equal to the distance between the lines). So you can pick any
points P1 and P2 that you like and the volume is determined by the sin of the angles
between the lines and the distance between the lines. If you divide the volume of the
parallelepiped by its base area, you get the height, which is the distance between the
lines. Cool!
x21: REAL ¬ line2.base.x-line1.base.x;
y21: REAL ¬ line2.base.y-line1.base.y;
z21: REAL ¬ line2.base.z-line1.base.z;
ab: REAL ¬ line1.axis.x*line2.axis.y-line2.axis.x*line1.axis.y;
bc: REAL ¬ line1.axis.y*line2.axis.z-line2.axis.y*line1.axis.z;
ca: REAL ¬ line1.axis.z*line2.axis.x-line2.axis.z*line1.axis.x;
den: REAL ¬ ab*ab+bc*bc+ca*ca;
IF den < 0.0001
THEN RETURN[Distance[line1.base, NearestToLine[line2, line1.base]]]
ELSE RETURN[ABS[x21*bc+y21*ca+z21*ab]/RealFns.SqRt[den]];
};
ClosestApproach2Lines: PUBLIC PROC [line1, line2: Ray] RETURNS [p1, p2: Triple] ~ {
This works, but ....
a, aa: REAL ← 1000000.0;
sense: BOOL ← TRUE;
DO
IF (sense ← NOT sense)
THEN p1 ← NearestToLine[line1, p2]
ELSE p2 ← NearestToLine[line2, p1];
IF (a ← Distance[p1, p2]) = 0.0 OR ABS[aa-a]/a < 0.0000001 THEN EXIT;
aa ← a;
ENDLOOP;
};
Simple Geometric Operations
V90:
PUBLIC
PROC [v0, v1: Triple, unitize:
BOOL ¬
TRUE]
RETURNS [t: Triple] ~ {
dot: REAL ¬ Dot[v0, v1];
t ¬ IF ABS[dot] > 0.000001 THEN [v1.x-dot*v0.x, v1.y-dot*v0.y, v1.z-dot*v0.z] ELSE v1;
IF unitize THEN t ¬ Unit[t];
};
Ortho:
PUBLIC
PROC [v: Triple, crosser: Triple ¬ [0.0, 0.0, 0.0]]
RETURNS [Triple] ~ {
length: REAL ¬ Length[v];
IF crosser = [0.0, 0.0, 0.0] THEN crosser ¬ [v.z, -v.x, v.y];
IF Parallel[crosser, v] THEN crosser ← [crosser.z, crosser.y, crosser.x];
RETURN[IF length = 0.0 THEN [0.0, 0.0, 0.0] ELSE Mul[Unit[Cross[v, crosser]], length]];
};
RotateAbout:
PUBLIC PROC [v, axis: Triple, a:
REAL, degrees:
BOOL ¬
TRUE]
RETURNS [t: Triple] ~ {
rotate: Matrix ¬ G3dMatrix.MakeRotate[axis, a, degrees, G3dMatrix.ObtainMatrix[]];
t ¬ G3dMatrix.TransformVec[v, rotate];
G3dMatrix.ReleaseMatrix[rotate];
};
Polar/Cartesian Coordinates
[Artwork node; type 'Artwork on' to command tool]
These procedures assume right-handed coordinate system: x to the right, y away, and z up.
The polar triple consists of longitude, latitude, and magnitude.
Latitude is deviation from the horizon; i.e., latitude = 90 is North Pole, -90 is South Pole.
Longitude is rotation about the vertical; i.e., longitude = 0 is the xAxis, 90 is the yAxis.
PolarFromCartesian:
PUBLIC
PROC [cartesian: Triple]
RETURNS [Triple] ~ {
xySum: REAL ¬ cartesian.x*cartesian.x+cartesian.y*cartesian.y;
lng: REAL ¬ RealFns.ArcTanDeg[cartesian.y, cartesian.x];
lat: REAL ¬ RealFns.ArcTanDeg[cartesian.z, RealFns.SqRt[xySum]];
mag: REAL ¬ RealFns.SqRt[xySum+cartesian.z*cartesian.z];
RETURN[[lng, lat, mag]];
};
CartesianFromPolar:
PUBLIC
PROC [polar: Triple]
RETURNS [Triple] ~ {
cosmag: REAL ¬ RealFns.CosDeg[polar.y]*polar.z;
RETURN[[
cosmag*RealFns.CosDeg[polar.x],
cosmag*RealFns.SinDeg[polar.x],
polar.z*RealFns.SinDeg[polar.y]]];
};
Miscellany
Clip:
PUBLIC
PROC [p: Triple, box: Box]
RETURNS [clipped: Triple] ~ {
clipped ¬ [
MIN[MAX[box.min.x, p.x], box.max.x],
MIN[MAX[box.min.y, p.y], box.max.y],
MIN[MAX[box.min.z, p.z], box.max.z]];
};
Project:
PUBLIC
PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
RETURN[IF v2 = origin THEN v2 ELSE Mul[v2, Dot[v1, v2]/Square[v2]]];
};
Average:
PUBLIC
PROC [triples: TripleSequence]
RETURNS [average: Triple] ~ {
average ¬ origin;
FOR n:
NAT
IN [0..triples.length)
DO
average ¬ Add[average, triples[n]];
ENDLOOP;
IF triples.length # 0 THEN average ¬ Div[average, triples.length];
};
CosineBetween:
PUBLIC
PROC [v0, v1: Triple, unitize:
BOOL ¬
FALSE]
RETURNS [r: REAL]
~ {
IF v0 = origin OR v1 = origin THEN RETURN[0.0];
r ¬ (v0.x*v1.x+v0.y*v1.y+v0.z*v1.z);
IF unitize
THEN r ¬
r/RealFns.SqRt[(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z)*(v1.x*v1.x+v1.y*v1.y+v1.z*v1.z)]
};
AngleBetween:
PUBLIC
PROC [
v0, v1: Triple,
degrees: BOOL ¬ TRUE,
unitize: BOOL ¬ FALSE]
RETURNS [REAL] ~ {
RETURN[
IF degrees
THEN G2dBasic.ArcCosDeg[CosineBetween[v0, v1, unitize]]
ELSE G2dBasic.ArcCos[CosineBetween[v0, v1, unitize]]];
};