Plane Operations
Normalize:
PUBLIC PROC [plane: Quad]
RETURNS [Quad] ~ {
length: REAL ← Vector3d.Length[[plane.x, plane.y, plane.z]];
IF length = 0.0 THEN ERROR Error[$NullVec, "null plane normal"];
RETURN[
IF length = 1.0
THEN plane
ELSE [plane.x/length, plane.y/length, plane.z/length, plane.w/length]];
};
FromPts:
PUBLIC PROC [p1, p2, p3: Triple]
RETURNS [Quad] ~ {
normal: Triple ← Vector3d.Cross[Vector3d.Sub[p2, p1], Vector3d.Sub[p3, p1]];
IF Vector3d.Null[normal] THEN ERROR Error[$NullVec, "points colinear"];
RETURN[[normal.x, normal.y, normal.z,
-(p1.x*(p2.y*p3.z-p3.y*p2.z)-p1.y*(p2.x*p3.z-p3.x*p2.z)+p1.z*(p2.x*p3.y-p3.x*p2.y))]];
};
FromPtNrm:
PUBLIC PROC
[point, normal: Triple]
RETURNS [Quad] ~ {
plane: Quad;
[[plane.x, plane.y, plane.z]] ← Vector3d.Normalize[normal];
plane.w ← -plane.x*point.x - plane.y*point.y - plane.z*point.z;
RETURN[plane];
};
IntWithLine:
PUBLIC
PROC
[plane: Quad, line: Line]
RETURNS [Triple] ~ {
alpha: REAL;
pdDot: REAL ← Vector3d.Dot[[plane.x, plane.y, plane.z], line.axis];
IF pdDot = 0.0 THEN ERROR Error[$ZeroDiv, "no intersection"];
alpha ← (-plane.w - Vector3d.Dot[[plane.x, plane.y, plane.z], line.base])/pdDot;
RETURN[Vector3d.Ray[line, alpha]];
};
TriArea:
PUBLIC PROC [p0, p1, p2: Triple]
RETURNS [
REAL] ~ {
RETURN[0.5*Vector3d.Length[Vector3d.Cross[Vector3d.Sub[p1, p0], Vector3d.Sub[p2, p0]]]];
};
CircleFromPts:
PUBLIC
PROC [p1, p2, p3: Triple]
RETURNS [Circle] ~ {
dot, t: REAL;
m1, m2, dm, center: Triple;
plane: Quad ← FromPts[p1, p2, p3];
v1: Triple ← Vector3d.Cross[Vector3d.Sub[p2, p1], [plane.x, plane.y, plane.z]];
v2: Triple ← Vector3d.Cross[Vector3d.Sub[p3, p2], [plane.x, plane.y, plane.z]];
v1 ← Vector3d.Normalize[v1];
v2 ← Vector3d.Normalize[v2];
IF (dot ← Vector3d.Dot[v1, v2]) = 0.0 THEN ERROR Error[$ZeroDiv, "points colinear"];
m1 ← Vector3d.Mul[Vector3d.Add[p1, p2], 0.5];
m2 ← Vector3d.Mul[Vector3d.Add[p2, p3], 0.5];
dm ← Vector3d.Sub[m2, m1];
t ← (dot*Vector3d.Dot[v1, dm]-Vector3d.Dot[v2, dm])/(1.0-dot*dot);
center ← Vector3d.Ray[[m1, v2], t];
RETURN[[center, Vector3d.Distance[center, p1]]];
};
IntOf3:
PUBLIC PROC [p1, p2, p3: Quad]
RETURNS [Triple] ~ {
Intersection is the determinant of a matrix formed from the three planes.
w: REAL ← p1.y*(p2.x*p3.z-p3.x*p2.z)-p1.x*(p2.y*p3.z-p3.y*p2.z)-p1.z*(p2.x*p3.y-p3.x*p2.y);
IF w = 0.0 THEN ERROR Error[$ZeroDiv, "no intersection"];
RETURN[[
(p1.y*(p2.z*p3.w-p3.z*p2.w)-p1.z*(p2.y*p3.w-p3.y*p2.w)+p1.w*(p2.y*p3.z-p3.y*p2.z))/w,
-(p1.x*(p2.z*p3.w-p3.z*p2.w)-p1.z*(p2.x*p3.w-p3.x*p2.w)+p1.w*(p2.x*p3.z-p3.x*p2.z))/w,
(p1.x*(p2.y*p3.w-p3.y*p2.w)-p1.y*(p2.x*p3.w-p3.x*p2.w)+p1.w*(p2.x*p3.y-p3.x*p2.y))/w]];
};
IntOf2:
PUBLIC PROC [plane1, plane2: Quad]
RETURNS [line: Line] ~ {
line.axis ← Vector3d.Cross[[plane1.x, plane1.y, plane1.z], [plane2.x, plane2.y, plane2.z]];
IF Vector3d.Null[line.axis] THEN ERROR Error[$NullVec, "no intersection"];
line.base ← IntOf3[plane1, plane2, [line.axis.x, line.axis.y, line.axis.z, 0.0]];
};
Center:
PUBLIC PROC [plane: Quad]
RETURNS [Triple] ~ {
RETURN[Vector3d.Mul[[plane.x, plane.y, plane.z], -plane.w]];
};
DistanceToPt:
PUBLIC PROC [point: Triple, plane: Quad]
RETURNS [
REAL] ~ {
length: REAL ← Vector3d.Length[[plane.x, plane.y, plane.z]];
IF length = 0.0 THEN ERROR Error[$NullVec, "null plane normal"];
RETURN[(point.x*plane.x + point.y*plane.y + point.z*plane.z + plane.w)/length];
};