Standard Geometric Operations on 3d vectors or points
Null:
PUBLIC
PROC [v: Triple]
RETURNS [
BOOL] ~ {
RETURN[v.x = 0.0 AND v.y = 0.0 AND v.z = 0.0];
};
Equal:
PUBLIC
PROC [v1, v2: Triple, tol:
REAL ← 0.001]
RETURNS [
BOOL] ~ {
RETURN[ABS[v1.x-v2.x]<tol AND ABS[v1.y-v2.y]<tol AND ABS[v1.z-v2.z] < tol];
};
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]];
};
Neg:
PUBLIC PROC [v: Triple]
RETURNS [Triple] ~ {
RETURN[[-v.x, -v.y, -v.z]];
};
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]];
};
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]];
};
Mag:
PUBLIC PROC [v: Triple]
RETURNS [
REAL] ~ {
RETURN[RealFns.SqRt[v.x*v.x+v.y*v.y+v.z*v.z]];
};
SameMag:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
magV2: REAL ← Mag[v2];
RETURN[IF magV2 # 0.0 THEN Mul[v2, Mag[v1]/magV2] ELSE v2];
};
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]];
};
LinePoint:
PUBLIC PROC [p: Triple, l: Line]
RETURNS [Triple] ~ {
lineV: Triple ← Normalize[l.axis];
RETURN[Add[l.base, Mul[lineV, Dot[Sub[p, l.base], lineV]]]];
};
Normalize:
PUBLIC PROC [v: Triple]
RETURNS [Triple] ~ {
m: REAL ~ RealFns.SqRt[v.x*v.x+v.y*v.y+v.z*v.z];
RETURN [IF m # 0.0 THEN [v.x/m, v.y/m, v.z/m] ELSE [v.x, v.y, v.z]];
};
Ray:
PUBLIC
PROC [l: Line, d:
REAL]
RETURNS [Triple] ~ {
RETURN[[l.base.x+d*l.axis.x, l.base.y+d*l.axis.y, l.base.z+d*l.axis.z]];
};
Project:
PUBLIC PROC [v1, v2: Triple]
RETURNS [Triple] ~ {
v2unit: Triple ← Normalize[v2];
RETURN[Mul[v2unit, Dot[v1, v2unit]]];
};
V90:
PUBLIC PROC [v0, v1: Triple]
RETURNS [Triple] ~ {
dot: REAL ← Dot[v0, v1];
RETURN [
IF
ABS[dot] > 0.000001
THEN Normalize[[v1.x-dot*v0.x, v1.y-dot*v0.y, v1.z-dot*v0.z]]
ELSE v1];
};
Ortho:
PUBLIC
PROC [v: Triple, crosser: Triple ← [0.0, 0.0, 0.0]]
RETURNS [Triple] ~ {
mag: REAL ← Mag[v];
IF Equal[crosser, v] THEN crosser ← [crosser.z, crosser.y, crosser.x];
RETURN[IF mag = 0.0 THEN [0.0, 0.0, 0.0] ELSE Mul[Normalize[Cross[v, crosser]], mag]];
};
RotateAbout:
PUBLIC PROC [v, axis: Triple, a:
REAL, degrees:
BOOL ←
TRUE]
RETURNS [Triple] ~ {
rotate: Matrix3d.Matrix ← Matrix3d.MakePureRotate[axis, a, degrees];
RETURN[Matrix3d.TransformVec[v, rotate]];
};
Collinear:
PUBLIC
PROC [p1, p2, p3: Triple, tol:
REAL ← 0.01]
RETURNS [
BOOL] ~ {
RETURN[Parallel[Sub[p1, p2], Sub[p2, p3], tol]];
};
VecsCoplanar:
PUBLIC PROC [v1, v2, v3: Triple, tol:
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] < tol];
};
PtsCoplanar:
PUBLIC PROC [p1, p2, p3, p4: Triple, tol:
REAL ← 0.01]
RETURNS [
BOOL] ~ {
RETURN[VecsCoplanar[Sub[p4, p1], Sub[p3, p1], Sub[p2, p1], tol]];
};
Parallel:
PUBLIC
PROC [v1, v2: Triple, tol:
REAL ← 0.005]
RETURNS [
BOOL] ~ {
par: BOOL ← FALSE;
temp1: Triple ← Normalize[v1];
temp2: Triple ← Normalize[v2];
RETURN[ABS[Dot[temp1, temp2]] > 1.-tol];
};
Perp:
PUBLIC
PROC [v1, v2: Triple, tol:
REAL ← 0.005]
RETURNS [
BOOL ←
FALSE] ~ {
perp: BOOL ← FALSE;
temp1: Triple ← Normalize[v1];
temp2: Triple ← Normalize[v2];
RETURN[ABS[Dot[temp1, temp2]] < tol];
};
PtOnLine:
PUBLIC
PROC [p: Triple, l: Line, tol:
REAL ← 0.005]
RETURNS [
BOOL] ~ {
RETURN[Distance[p, LinePoint[p, l]] < tol];
};