File: SVVector3dImpl.mesa
Author: Eric Bier before January 12, 1983 1:35 pm
Last edited by Bier on May 29, 1987 5:16:25 pm PDT
Contents: Vector3d addition and multiplication and stuff like that.
DIRECTORY
RealFns, SV2d, SV3d, SVVector3d;
SVVector3dImpl:
CEDAR PROGRAM
IMPORTS RealFns
EXPORTS SVVector3d =
BEGIN
Point3d: TYPE = SV3d.Point3d;
Vector3d: TYPE = SV3d.Vector3d;
Vector2d: TYPE = SV2d.Vector2d;
galmostZero: REAL = 1.0e-10; -- Must be less than 1.0e-8 I've discovered. This is an old comment.
Not yet tested. Not used anywhere yet.
VectorToPlane:
PRIVATE
PROC [v: Vector3d, xaxis: Vector3d, normal: Vector3d]
RETURNS [v2: Vector2d] = {
Find a two-dimensional vector v2 with the same length as v, such that the angle between v2 and the x-axis in 2-space is the same as the angle between v and xaxis in 3-space (where the positive sense of the angle is counter-clockwise around "normal"). Two methods to try:
1) v xaxis = |v| |xaxis| cosq. |xaxis v| = |v||xaxis| sinq. The ratio = tanq so we can easily find q and then use VectorFromAngle in two dimensions.
2) Normalize xaxis. Then v xaxis = |v| cosq, |xaxis v|= |v| sinq. These are the two components of v2.
The second method seems the more efficient since no ArcTan is involved. Two square roots are involved to normalize xaxis and find the magnitude of |v xaxis|.
dotProd, crossProdMag: REAL;
crossProd: Vector3d;
xaxis ← Normalize[xaxis];
dotProd ← DotProduct[v, xaxis];
crossProd ← CrossProduct[xaxis, v];
Does this have the same sense or opposite sense to normal? If opposite, then change the sign of the dot product.
IF NOT SameSense[crossProd, normal] THEN dotProd ← -dotProd;
crossProdMag ← Magnitude[crossProd];
v2 ← [dotProd, crossProdMag];
};
Not yet implemented.
SameSense:
PUBLIC
PROC [v1, v2: Vector3d]
RETURNS [
BOOL] = {
Are v1 and v2 more nearly parallel or anti-parallel? Taking the sign of the dot product would work but is more expensive then needed. Compare the signs of the components and let the majority vote win.
RETURN[TRUE];
};
VectorFromPlane:
PUBLIC
PROC [v2: Vector2d, xaxis: Vector3d, normal: Vector3d]
RETURNS [v: Vector3d] = {
The inverse of VectorToPlane. Given a vector in 2-space, and a plane p and x-axis vector in 3-space, find the vector v in plane p whose angle with xaxis is the same as the angle of the absolute angle of v2 in 2-space.
Method: We know that v normal = 0, we know the value of v xaxis and we know the desired length of v. Let normal = [a,b,c], v = [x,y,z], xaxis = [p,q,r]. Let v2[1] (=|v| cosq ) be called A. Then we know:
ax + by + cz = 0.
px + qy + rz = A.
};
Not yet implemented.
VectorPlusAngleInPlane:
PUBLIC
PROC [v: Vector3d, degrees:
REAL, normal: Vector3d]
RETURNS [rotatedV: Vector2d] = {
Like SVVector2d.VectorPlusAngle, but we must specify a plane to rotate in. "normal" is a vector normal to such a plane (and must be normal to v as well for this operation to make sense. The sense of normal determines which rotational direction is counter-clockwise by the right-hand rule. We proceed as follows:
};
Add:
PUBLIC
PROC [v1: Vector3d, v2: Vector3d]
RETURNS [sumV: Vector3d] = {
sumV[1] ← v1[1] + v2[1];
sumV[2] ← v1[2] + v2[2];
sumV[3] ← v1[3] + v2[3];
};
Sub:
PUBLIC
PROC [v1: Vector3d, v2: Vector3d]
RETURNS [v1Minusv2: Vector3d] = {
v1Minusv2[1] ← v1[1] - v2[1];
v1Minusv2[2] ← v1[2] - v2[2];
v1Minusv2[3] ← v1[3] - v2[3];
};
Scale:
PUBLIC
PROC [v: Vector3d, scalar:
REAL]
RETURNS [scaledV: Vector3d] = {
scaledV[1] ← v[1]*scalar; scaledV[2] ← v[2]*scalar; scaledV[3] ← v[3]*scalar;
};
Normalize:
PUBLIC
PROC [v: Vector3d]
RETURNS [normV: Vector3d] = {
Returns the unit vector with the same direction as v.
mag: REAL ← Magnitude[v];
normV[1] ← v[1]/mag;
normV[2] ← v[2]/mag;
normV[3] ← v[3]/mag;
};
Negate:
PUBLIC
PROC [v: Vector3d]
RETURNS [negV: Vector3d] = {
negV[1] ← -v[1]; negV[2] ← -v[2]; negV[3] ← -v[3];
};
DotProduct:
PUBLIC
PROC [v1: Vector3d, v2: Vector3d]
RETURNS [scalar:
REAL] = {
scalar ← v1[1]*v2[1] + v1[2]*v2[2] + v1[3]*v2[3];
};
ElementwiseProduct:
PUBLIC
PROC [v1, v2: Vector3d]
RETURNS [prod: Vector3d] = {
prod[1] ← v1[1] * v2[1];
prod[2] ← v1[2] * v2[2];
prod[3] ← v1[3] * v2[3];
};
CrossProduct:
PUBLIC
PROC [v1: Vector3d, v2: Vector3d]
RETURNS [prodV: Vector3d] = {
| i j k |
| v1x v1y v1z |=(v1y*v2z - v1z*v2y) i + (v1z*v2x - v1x*v2z) j
| v2x v2y v2z |(v1x*v2y - v1y*v2x) k
prodV[1] ← v1[2]*v2[3] - v1[3]*v2[2];
prodV[2] ← v1[3]*v2[1] - v1[1]*v2[3];
prodV[3] ← v1[1]*v2[2] - v1[2]*v2[1];
};
AngleCCWBetweenVectors:
PUBLIC
PROC [v1: Vector3d, v2: Vector3d]
RETURNS [degrees:
REAL] = {
The cross product v1 x v2 produces a vector. With the plane determined by v1 and v2 facing you, measure the counter-clockwise angle from v1 to v2. Note that, with this definition, 0 <= degress < 180, so sin(q) is always positive.
v1hat, v2hat, cross: Vector3d;
sin, cos: REAL;
v1hat ← Normalize[v1];
v2hat ← Normalize[v2];
cross ← CrossProduct[v1hat, v2hat];
sin ← Magnitude[cross];
cos ← DotProduct[v1hat, v2hat];
degrees ← RealFns.ArcTanDeg[sin, cos];
};
Magnitude:
PUBLIC
PROC [v: Vector3d]
RETURNS [magnitude:
REAL] = {
magnitude ← RealFns.SqRt[(v[1]*v[1]+v[2]*v[2]+v[3]*v[3])];
};
Distance:
PUBLIC
PROC [p1, p2: Point3d]
RETURNS [dist:
REAL] = {
dist ← Magnitude[Sub[p2, p1]];
};
MagnitudeSquared:
PUBLIC PROC [v: Vector3d]
RETURNS [magSquared:
REAL]= {
magSquared ← (v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
};
DistanceSquared:
PUBLIC PROC [p1, p2: Point3d]
RETURNS [distSquared:
REAL] = {
distSquared ← MagnitudeSquared[Sub[p2, p1]];
};
VectorFromPoints:
PUBLIC PROC [tail, head: Point3d]
RETURNS [vector: Vector3d] = {
vector[1] ← head[1] - tail[1];
vector[2] ← head[2] - tail[2];
vector[3] ← head[3] - tail[3];
};
AlmostZero:
PRIVATE
PROC [r:
REAL]
RETURNS [
BOOL] = {
RETURN[ABS[r] < galmostZero];
};
AlmostEqual:
PRIVATE
PROC [a, b:
REAL]
RETURNS [
BOOL] = {
RETURN[ABS[(a-b)/a] < 0.01];
};
Parallel: PUBLIC PROC [v1, v2: Vector3d] RETURNS [BOOL] = {
This implementation is a complete disaster.
a,b,c: REAL;
IF AlmostZero[v2[1]] THEN {
IF NOT AlmostZero[v1[1]] THEN RETURN[FALSE] ELSE a ← 0}
ELSE { -- v2[1] # 0
IF AlmostZero[v1[1]] THEN RETURN[FALSE] ELSE a ← v1[1]/v2[1]};
IF AlmostZero[v2[2]] THEN {
IF NOT AlmostZero[v1[2]] THEN RETURN[FALSE] ELSE b ← 0}
ELSE { -- v2[2] # 0
IF AlmostZero[v1[2]] THEN RETURN[FALSE] ELSE b ← v1[2]/v2[2]};
IF AlmostZero[v2[3]] THEN {
IF NOT AlmostZero[v1[3]] THEN RETURN[FALSE] ELSE c ← 0}
ELSE { -- v2[3] # 0
IF AlmostZero[v1[3]] THEN RETURN[FALSE] ELSE c ← v1[3]/v2[3]};
RETURN[AllAlmostEqual[a, b, c]];
};
Parallel:
PUBLIC
PROC [v1, v2: Vector3d]
RETURNS [
BOOL] = {
I have seen this crossproduct be as high as 1.058926e-7 for vectors that should be parallel.
crossProd: REAL ← Magnitude[CrossProduct[Normalize[v1], Normalize[v2]]];
answer: BOOL;
answer ← ABS[crossProd] < 1.0E-5;
RETURN[answer];
};
AllEqual: PRIVATE PROC [a, b, c: REAL] RETURNS [BOOL] = {
IF a = 0 THEN {
IF b = 0 THEN RETURN[TRUE]-- both vectors have only a z component
ELSE IF c = 0 THEN RETURN[TRUE] -- both vectors have only a y component
ELSE RETURN[b=c];
}
ELSE {
IF b = 0 THEN {
IF c = 0 THEN RETURN[TRUE]-- both vectors have only an x component
ELSE RETURN[a=c];
}
ELSE {
IF c = 0 THEN RETURN[a=b]
ELSE RETURN[a=b AND b = c];
};
};
}; -- end of AllEqual
AllAlmostEqual: PRIVATE PROC [a, b, c: REAL] RETURNS [BOOL] = {
Was used in the unsuccessful form of Parallel above.
almostZero: REAL ← 1.0e-10;
IF AlmostZero[a] THEN {
IF AlmostZero[b] THEN RETURN[TRUE] -- both vectors have only a z component
ELSE IF AlmostZero[c] THEN RETURN[TRUE] -- both vectors have only a y component
ELSE RETURN[AlmostEqual[b, c]];
}
ELSE {
IF AlmostZero[b] THEN {
IF AlmostZero[c] THEN RETURN[TRUE] -- both vectors have only an x component
ELSE RETURN[AlmostEqual[a, c]];
}
ELSE {
IF AlmostZero[c] THEN RETURN[AlmostEqual[a, b]]
ELSE RETURN[AlmostEqual[a, b] AND AlmostEqual[b, c]];
};
};
}; -- end of AllAlmostEqual
Perpendicular:
PUBLIC
PROC [v1, v2: Vector3d]
RETURNS [
BOOL] = {
RETURN[DotProduct[v1, v2] = 0];
};
Vector2DAsXYVector:
PUBLIC
PROC [vXY: Vector2d]
RETURNS [vZeroZ: Vector3d] = {
vZeroZ[1] ← vXY[1];
vZeroZ[2] ← vXY[2];
vZeroZ[3] ← 0;
};
Vector2DAsYZVector:
PUBLIC
PROC [vYZ: Vector2d]
RETURNS [vZeroX: Vector3d] = {
vZeroX[1] ← 0;
vZeroX[2] ← vYZ[1];
vZeroX[3] ← vYZ[2];
};
Vector2DAsZXVector:
PUBLIC
PROC [vZX: Vector2d]
RETURNS [vZeroY: Vector3d] = {
vZeroY[1] ← vZX[2];
vZeroY[2] ← 0;
vZeroY[3] ← vZX[1];
};
ProjectOntoXYPlane:
PUBLIC
PROC [v: Vector3d]
RETURNS [v2d: Vector2d] = {
v2d[1] ← v[1];
v2d[2] ← v[2];
};
ProjectOntoYZPlane:
PUBLIC
PROC [v: Vector3d]
RETURNS [v2d: Vector2d] = {
v2d[1] ← v[2];
v2d[2] ← v[3];
};
ProjectOntoZXPlane:
PUBLIC
PROC [v: Vector3d]
RETURNS [v2d: Vector2d] = {
v2d[1] ← v[3];
v2d[2] ← v[1];
};
END.