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. VectorFromAngles: PUBLIC PROC [azimuth, slope: REAL] RETURNS [vector: Vector3d] = { sinq, cosq, sinf, cosf: REAL; sinq _ RealFns.SinDeg[azimuth]; cosq _ RealFns.CosDeg[azimuth]; sinf _ RealFns.SinDeg[slope]; cosf _ RealFns.CosDeg[slope]; vector[1] _ cosf*cosq; vector[3] _ -cosf*sinq; vector[2] _ sinf; }; VectorToPlane: PRIVATE PROC [v: Vector3d, xaxis: Vector3d, normal: Vector3d] RETURNS [v2: Vector2d] = { dotProd, crossProdMag: REAL; crossProd: Vector3d; xaxis _ Normalize[xaxis]; dotProd _ DotProduct[v, xaxis]; crossProd _ CrossProduct[xaxis, v]; IF NOT SameSense[crossProd, normal] THEN dotProd _ -dotProd; crossProdMag _ Magnitude[crossProd]; v2 _ [dotProd, crossProdMag]; }; SameSense: PUBLIC PROC [v1, v2: Vector3d] RETURNS [BOOL] = { RETURN[TRUE]; }; VectorFromPlane: PUBLIC PROC [v2: Vector2d, xaxis: Vector3d, normal: Vector3d] RETURNS [v: Vector3d] = { }; VectorPlusAngleInPlane: PUBLIC PROC [v: Vector3d, degrees: REAL, normal: Vector3d] RETURNS [rotatedV: Vector2d] = { }; 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] = { 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] = { 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] = { 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] = { crossProd: REAL _ Magnitude[CrossProduct[Normalize[v1], Normalize[v2]]]; answer: BOOL; answer _ ABS[crossProd] < 1.0E-5; RETURN[answer]; }; 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. \File: SVVector3dImpl.mesa Author: Eric Bier before January 12, 1983 1:35 pm Last edited by Bier on July 8, 1987 2:02:25 pm PDT Contents: Vector3d addition and multiplication and stuff like that. azimuth and slope must be in degrees in the range: -180 < angle <= 180. vector is a unit vector. Not yet tested. Not used anywhere yet. 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 o xaxis = |v| |xaxis| cosq. |xaxis X 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 o xaxis = |v| cosq, |xaxis X 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 X xaxis|. Does this have the same sense or opposite sense to normal? If opposite, then change the sign of the dot product. Not yet implemented. 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. Not yet implemented. 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 o normal = 0, we know the value of v o 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. 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: Returns the unit vector with the same direction as v. | i j k | | v1x v1y v1z |=(v1y*v2z - v1z*v2y) i + (v1z*v2x - v1x*v2z) j | v2x v2y v2z |(v1x*v2y - v1y*v2x) k 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. 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]]; }; I have seen this crossproduct be as high as 1.058926e-7 for vectors that should be parallel. 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 Э╩Ц "cedar" styleШIheadЪЬЩIprocЪЬ1Щ1LЪЬ2Щ2LЪЬCЩCLШЪ╧k Ш LЪЬ Ш LШЧЪЬЭ ШLЪЭЬШLЪЭЬ ШЧLШLЪЭШШLЪЬ ЭЬШLЪЬ ЭЬШLЪЬ ЭЬШLШLЪЬ ЭЬQШbLШЧcodeЪ ╧nЬЭЬЭЬЭЬЭЬШSMЪЬGЩGMЪЬЩMЪЬ╧gЬЯЬЯЬЯЬЭЬШMЪЬЯЬШMЪЬЯЬШMЪЬЯЬШMЪЬЯЬШMЪЬЯЬЯЬШMЪЬЯЬЯЬШMЪЬЯЬШMЪЬШMШЧЪЮ ЬЭЬЭЬ2ЭЬШgLЩ'LЪЬПЩПLЪЬ╧mЬЯЬаЬЯЬЯЬЯЬЯЬ0ЩЬLЪЬаЬЯЬаЬЯЬЯЬ%ЩmLЪЬФЯЬаЬЯЬЩаLЪЬЭЬШLЪЬШLЪЬШLЪЬШЪЬ#Ш#LЪЬqЩqЧLЪЭЬЭЬЭЬШLЪЬ4Ш4LЪЬШЧЪ Ю ЬЭЬЭЬЭЬ ЭЬШOLЪЬ1Ш1LЪЬШЧЪЮЬЭЬЭЬЭЬШOLЪЬШLЪЬШLЪЬШLЪЬШLШЧЪЮЬЭЬЭЬЭЬШTLЪЬЩLЪЬ=Щ=LЪЬ$Щ$LЪЬ%Ш%LЪЬ%Ш%LЪЬ%Ш%LЪЬШLШЧЪ ЮЬЭЬЭЬЭЬЭЬШ\LЪЬ╤ЯЬЩчLЪЬШLЪЬ ЭЬШLЪЬШLЪЬШLЪЬ#Ш#LШLЪЬШLЪЬ&Ш&LШЧLШЪ Ю ЬЭЬЭЬЭЬ ЭЬШBLЪЬ:Ш:LЪЬШLШЧЪ ЮЬЭЬЭЬЭЬЭЬШ@LЪЬШLШЧLШЪЮЬЭЬЭЬЭЬШILЪЬ-Ш-LЪЬШЧЪЮЬЭЬЭЬЭЬШNLЪЬ,Ш,LШЧLШЪЮЬЭЬЭЬШRLЪЬШLЪЬШLЪЬШLЪЬШLШЧLШЪЮ ЬЭЬЭЬЭЬЭЬЭЬШ5LЪЭЬЭЬШLШLШЧЪЮЬЭЬЭЬЭЬЭЬЭЬШ9LЪЭЬЭЬШLШLШЧЪ ЮЬЭЬЭЬЭЬЭЬЩ;LЩ+LЪЬЭЬЩЪЭЬЭЬЩLЪЭЬЭЬЭЬЭЬЭЬЭЬЩ7ЧЪЭЬ╧cЩLЪ ЭЬЭЬЭЬЭЬЭЬЩ>LЩЧЪЭЬЭЬЩLЪЭЬЭЬЭЬЭЬЭЬЭЬЩ7ЧЪЭЬбЩLЪ ЭЬЭЬЭЬЭЬЭЬЩ>LЩЧЪЭЬЭЬЩLЪЭЬЭЬЭЬЭЬЭЬЭЬЩ7ЧЪЭЬбЩLЪ ЭЬЭЬЭЬЭЬЭЬЩ>ЧLЪЭЬЩ LЪЬЩLЩЧЪ ЮЬЭЬЭЬЭЬЭЬШ;LЩ\LЪЬЭЬ9ШHLЪЬЭЬШ LЪЬ ЭЬШ!LЪЭЬ ШLЪЬШLШЧЪЮЬЭЬЭЬЭЬЭЬЭЬЩ9ЪЭЬЭЬЩLЪ ЭЬЭЬЭЬЭЬб'ЩALЪЬЭЬЭЬЭЬЭЬЭЬб'ЩKLЪЬЭЬЭЬЩLЪЬЩЧЪЭЬЩЪЭЬЭЬЩLЪ ЭЬЭЬЭЬЭЬб(ЩBLЪЭЬЭЬЩLЪЬЩЧЪЭЬЩLЪЭЬЭЬЭЬЩLЪЭЬЭЬЭЬЩLЪЬЩЧLЪЬЩЧLЪЬбЩLЩЧЪЮЬЭЬЭЬЭЬЭЬЭЬЩ?LЩ4LЪЬЭЬЩЪЭЬЭЬЩLЪ ЭЬЭЬЭЬЭЬб(ЩJLЪЬЭЬЭЬЭЬЭЬЭЬб'ЩRLЪЬЭЬЭЬЩ"LЪЬЩЧЪЭЬЩЪЭЬЭЬЩLЪ ЭЬЭЬЭЬЭЬб)ЩKLЪЭЬЭЬЩLЪЬЩЧЪЭЬЩLЪЭЬЭЬЭЬЩ/LЪЭЬЭЬЭЬЩ5LЪЬЩЧLЪЬЩЧLЪЬб ЬЩLЩЧЪ Ю ЬЭЬЭЬЭЬЭЬШ@LЪЭЬШLЪЬШLШЧLШЪЮЬЭЬЭЬЭЬШNLЪЬШLЪЬШLЪЬШLЪЬШЧЪЮЬЭЬЭЬЭЬШNLЪЬШLЪЬШLЪЬШLЪЬШЧЪЮЬЭЬЭЬЭЬШNLЪЬШLЪЬШLЪЬШLЪЬШLШЧLШЪЮЬЭЬЭЬЭЬШILЪЬШLЪЬШLЪЬШЧЪЮЬЭЬЭЬЭЬШILЪЬШLЪЬШLЪЬШЧЪЮЬЭЬЭЬЭЬШILЪЬШLЪЬШLЪЬШLШЧLШLЪЭЬШLШLШLШЧЕЧ▓/