1 /* Copyright (C) 1992-1998 The Geometry Center
2  * Copyright (C) 1998-2000 Stuart Levy, Tamara Munzner, Mark Phillips
3  *
4  * This file is part of Geomview.
5  *
6  * Geomview is free software; you can redistribute it and/or modify it
7  * under the terms of the GNU Lesser General Public License as published
8  * by the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Geomview is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with Geomview; see the file COPYING.  If not, write
18  * to the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139,
19  * USA, or visit http://www.gnu.org.
20  */
21 
22 /*
23 **	hpoint3.h - procedural interface to 3D point geometry
24 **
25 **	pat hanrahan
26 */
27 
28 /* Authors: Charlie Gunn, Pat Hanrahan, Stuart Levy, Tamara Munzner, Mark Phillips */
29 
30 #ifndef _GV_HPOINT3_H_
31 #define _GV_HPOINT3_H_
32 
33 #if HAVE_CONFIG_H
34 # include "config.h"
35 #endif
36 
37 #if 0
38 static char copyright[] = "Copyright (C) 1992-1998 The Geometry Center\n\
39 Copyright (C) 1998-2000 Stuart Levy, Tamara Munzner, Mark Phillips";
40 #endif
41 
42 #include <math.h>
43 #include "tolerance.h"
44 
45 /* for safety we make forward declarations for all functions defined *
46  * here, only then we include other header files containing inline
47  * functions, they will follow the same scheme and everything will
48  * work.
49  */
50 #include "geomtypes.h"
51 
52 static inline HPoint3 *HPt3Create(void);
53 static inline void HPt3Delete(HPoint3 *pt);
54 static inline void HPt3Print(HPoint3 *pt);
55 static inline void HPt3Copy(HPoint3 *pt1, HPoint3 *pt2);
56 static inline void HPt3Add(HPoint3 *pt1, HPoint3 *pt2, HPoint3 *pt3);
57 static inline void
58 HPt3From(HPoint3 *pt, HPt3Coord x, HPt3Coord y, HPt3Coord z, HPt3Coord w);
59 static inline int
60 HPt3From3HPl3s(HPoint3 *pt, HPlane3 *pl1, HPlane3 *pl2, HPlane3 *pl3);
61 static inline int HPt3From2HLn3s( HPoint3 *pt, HLine3 *ln1, HLine3 *ln2 );
62 static inline int HPt3IntersectHPt3( HPoint3 *pt1, HPoint3 *pt2, HLine3 *ln );
63 static inline void
64 HPt3Pencil(HPt3Coord t1, HPoint3 *pt1,
65 	   HPt3Coord t2, HPoint3 *pt2, HPoint3 *pt3 );
66 static inline HPt3Coord HPt3DotHPl3(HPoint3 *pt, HPlane3 *pl);
67 static inline int HPt3Compare(HPoint3 *pt1, HPoint3 *pt2);
68 static inline int HPt3Undefined(HPoint3 *pt);
69 static inline int HPt3Infinity(HPoint3 *pt);
70 static inline int HPt3CoincidentHPl3(HPoint3 *pt, HPlane3 *pl);
71 static inline int HPt3CoincidentHLn3(HPoint3 *pt, HLine3 *ln);
72 static inline int HPt3CoincidentHPt3(HPoint3 *pt1, HPoint3 *pt2);
73 static inline void HPt3Transform(Transform3 T, HPoint3 *pt1, HPoint3 *pt2);
74 static inline int
75 HPt3TransformN(Transform3 T, HPoint3 *pt1, HPoint3 *pt2, int n);
76 static inline HPt3Coord HPt3TransPt3(Transform3 T, HPoint3 *pin, Point3 *pout);
77 static inline void Pt3ToHPt3(Point3 *v3, HPoint3 *v4, int n);
78 static inline void HPt3ToPt3(HPoint3 *hp, Point3 *p);
79 static inline void Pt4ToHPt3(HPoint3 *pt4, int *axes, HPoint3 *hp3);
80 static inline void HPt3Dehomogenize(HPoint3 *hp1, HPoint3 *hp2);
81 static inline void HPt3Dual(HPoint3 *pt, HPlane3 *pl);
82 static inline void
83 HPt3LinSum(HPt3Coord scale1, HPoint3 *in1,
84 	   HPt3Coord scale2, HPoint3 *in2, HPoint3 *out);
85 static inline void
86 HPt3LinSumDenorm(HPt3Coord scale1, HPoint3 *in1,
87 		 HPt3Coord scale2, HPoint3 *in2, HPoint3 *out);
88 static inline void HPt3SizeOne(HPoint3 *pt, HPoint3 *out);
89 static inline HPt3Coord HPt3R40Dot(HPoint3 *a, HPoint3 *b);
90 static inline HPt3Coord HPt3R31Dot(HPoint3 *a, HPoint3 *b);
91 static inline HPt3Coord HPt3R30Dot(HPoint3 *a, HPoint3 *b);
92 static inline HPt3Coord HPt3SpaceDot(HPoint3 *a, HPoint3 *b, int space);
93 static inline HPt3Coord HPt3DotPt3(HPoint3 *a, Point3 *b);
94 static inline void HPt3R40Normalize(HPoint3 *a);
95 static inline void HPt3R31Normalize(HPoint3 *a);
96 static inline void HPt3R30Normalize(HPoint3 *a);
97 static inline void HPt3SpaceNormalize(HPoint3 *a, int space);
98 static inline HPt3Coord HPt3HypDistance(HPoint3 *a, HPoint3 *b);
99 static inline HPt3Coord HPt3Distance(HPoint3 *a, HPoint3 *b);
100 static inline HPt3Coord HPt3SphDistance(HPoint3 *a, HPoint3 *b);
101 static inline HPt3Coord HPt3SpaceDistance(HPoint3 *a, HPoint3 *b, int space);
102 static inline void HPt3Sub(HPoint3 *a, HPoint3 *b, HPoint3 *aminusb);
103 static inline void HPt3Scale(HPt3Coord s, HPoint3 *a, HPoint3 *sa);
104 static inline void HPt3GramSchmidt(HPoint3 *base, HPoint3 *v);
105 static inline void HPt3HypGramSchmidt(HPoint3 *base, HPoint3 *v);
106 static inline void HPt3SphGramSchmidt(HPoint3 *base, HPoint3 *v);
107 static inline void HPt3SpaceGramSchmidt(HPoint3 *base, HPoint3 *v, int space);
108 #if 0
109 static inline HPt3Coord
110 HPt3Angle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2);
111 static inline HPt3Coord
112 HPt3HypAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2);
113 static inline HPt3Coord
114 HPt3SphAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2);
115 static inline HPt3Coord
116 HPt3SpaceAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2, int space);
117 #endif
118 static inline void HPt3SubPt3(HPoint3 *p1, HPoint3 *p2, Point3 *v);
119 static inline void HPt3MinMax(HPoint3 *min, HPoint3 *max, HPoint3 *other);
120 static inline void Pt3MinMax(HPoint3 *min, HPoint3 *max, HPoint3 *other);
121 static inline void Pt4MinMax(HPoint3 *min, HPoint3 *max, HPoint3 *other);
122 
123 #include "hg4.h"
124 #include "hline3.h"
125 #include "hplane3.h"
126 #include "point3.h"
127 
HPt3Create(void)128 static inline HPoint3 *HPt3Create(void)
129 {
130   return (HPoint3 *) Hg4Create();
131 }
132 
133 static inline void
HPt3Delete(HPoint3 * pt)134 HPt3Delete( HPoint3 *pt )
135 {
136   Hg4Delete( (Hg4Tensor1Ptr) pt );
137 }
138 
139 static inline void
HPt3Print(HPoint3 * pt)140 HPt3Print( HPoint3 *pt )
141 {
142   Hg4Print( (Hg4Tensor1Ptr) pt );
143 }
144 
HPt3Copy(HPoint3 * pt1,HPoint3 * pt2)145 static inline void HPt3Copy(HPoint3 *pt1, HPoint3 *pt2)
146 {
147   *pt2 = *pt1;
148 }
149 
150 static inline void
HPt3Add(HPoint3 * pt1,HPoint3 * pt2,HPoint3 * pt3)151 HPt3Add(HPoint3 *pt1, HPoint3 *pt2, HPoint3 *pt3)
152 {
153   Hg4Add( (Hg4Tensor1Ptr) pt1, (Hg4Tensor1Ptr) pt2 , (Hg4Tensor1Ptr) pt3);
154 }
155 
156 static inline void
HPt3From(HPoint3 * pt,HPt3Coord x,HPt3Coord y,HPt3Coord z,HPt3Coord w)157 HPt3From(HPoint3 *pt, HPt3Coord x, HPt3Coord y, HPt3Coord z, HPt3Coord w)
158 {
159   Hg4From( (Hg4Tensor1Ptr)pt, x, y, z, w );
160 }
161 
162 static inline int
HPt3From3HPl3s(HPoint3 * pt,HPlane3 * pl1,HPlane3 * pl2,HPlane3 * pl3)163 HPt3From3HPl3s( HPoint3 *pt, HPlane3 *pl1, HPlane3 *pl2, HPlane3 *pl3 )
164 {
165   return Hg4Intersect3(
166 		       (Hg4Tensor1Ptr)pl1, (Hg4Tensor1Ptr)pl2, (Hg4Tensor1Ptr)pl3,
167 		       (Hg4Tensor1Ptr)pt, 1 );
168 }
169 
170 static inline int
HPt3From2HLn3s(HPoint3 * pt,HLine3 * ln1,HLine3 * ln2)171 HPt3From2HLn3s( HPoint3 *pt, HLine3 *ln1, HLine3 *ln2 )
172 {
173   HPlane3 pl;
174 
175   return HLn3IntersectHLn3( ln1, ln2, &pl, pt );
176 }
177 
178 static inline int
HPt3IntersectHPt3(HPoint3 * pt1,HPoint3 * pt2,HLine3 * ln)179 HPt3IntersectHPt3( HPoint3 *pt1, HPoint3 *pt2, HLine3 *ln )
180 {
181   return HLn3From2HPt3s( ln, pt1, pt2 );
182 }
183 
184 static inline void
HPt3Pencil(HPt3Coord t1,HPoint3 * pt1,HPt3Coord t2,HPoint3 * pt2,HPoint3 * pt3)185 HPt3Pencil( HPt3Coord t1, HPoint3 *pt1, HPt3Coord t2, HPoint3 *pt2, HPoint3 *pt3 )
186 {
187   Hg4Pencil( t1, (Hg4Tensor1Ptr)pt1,
188 	     t2, (Hg4Tensor1Ptr)pt2, (Hg4Tensor1Ptr)pt3);
189 }
190 
191 static inline HPt3Coord
HPt3DotHPl3(HPoint3 * pt,HPlane3 * pl)192 HPt3DotHPl3( HPoint3 *pt, HPlane3 *pl )
193 {
194   return Hg4ContractPiQi( (Hg4Tensor1Ptr)pt, (Hg4Tensor1Ptr)pl );
195 }
196 
197 static inline int
HPt3Compare(HPoint3 * pt1,HPoint3 * pt2)198 HPt3Compare( HPoint3 *pt1, HPoint3 *pt2 )
199 {
200   return Hg4Compare( (Hg4Tensor1Ptr)pt1, (Hg4Tensor1Ptr)pt2 );
201 }
202 
203 static inline int
HPt3Undefined(HPoint3 * pt)204 HPt3Undefined( HPoint3 *pt )
205 {
206   return Hg4Undefined( (Hg4Tensor1Ptr)pt );
207 }
208 
209 static inline int
HPt3Infinity(HPoint3 * pt)210 HPt3Infinity( HPoint3 *pt )
211 {
212   return Hg4Infinity( (Hg4Tensor1Ptr)pt, 0 );
213 }
214 
215 static inline int
HPt3CoincidentHPl3(HPoint3 * pt,HPlane3 * pl)216 HPt3CoincidentHPl3( HPoint3 *pt, HPlane3 *pl )
217 {
218   return fzero(HPt3DotHPl3(pt,pl));
219 }
220 
221 static inline int
HPt3CoincidentHLn3(HPoint3 * pt,HLine3 * ln)222 HPt3CoincidentHLn3( HPoint3 *pt, HLine3 *ln )
223 {
224   HPlane3 pl;
225 
226   return HLn3IntersectHPt3( ln, pt, &pl );
227 }
228 
229 static inline int
HPt3CoincidentHPt3(HPoint3 * pt1,HPoint3 * pt2)230 HPt3CoincidentHPt3( HPoint3 *pt1, HPoint3 *pt2 )
231 {
232   return Hg4Coincident( (Hg4Tensor1Ptr)pt1, (Hg4Tensor1Ptr)pt2 );
233 }
234 
235 /*
236  * pt2 = pt1 * T
237  */
238 static inline void
HPt3Transform(Transform3 T,HPoint3 * pt1,HPoint3 * pt2)239 HPt3Transform(Transform3 T, HPoint3 *pt1, HPoint3 *pt2)
240 {
241   HPt3Coord x = pt1->x, y = pt1->y, z = pt1->z, w = pt1->w;
242 
243   pt2->x = x*T[0][0] + y*T[1][0] + z*T[2][0] + w*T[3][0];
244   pt2->y = x*T[0][1] + y*T[1][1] + z*T[2][1] + w*T[3][1];
245   pt2->z = x*T[0][2] + y*T[1][2] + z*T[2][2] + w*T[3][2];
246   pt2->w = x*T[0][3] + y*T[1][3] + z*T[2][3] + w*T[3][3];
247 }
248 
249 static inline int
HPt3TransformN(Transform3 T,HPoint3 * pt1,HPoint3 * pt2,int n)250 HPt3TransformN(Transform3 T, HPoint3 *pt1, HPoint3 *pt2, int n)
251 {
252   int res = 0;
253 
254   while (--n >= 0) {
255     HPt3Transform(T, pt1++, pt2);
256     if ((pt2++)->w != 1.0) {
257       res = 1;
258     }
259   }
260   return res;
261 }
262 
263 /*
264  * Transform and project an HPoint3 onto a plain Point3.
265  * Transforms pin . T -> pout,
266  * then projects pout.{x,y,z} /= pout.w.
267  * Returns pout.w.
268  */
269 static inline HPt3Coord
HPt3TransPt3(Transform3 T,HPoint3 * pin,Point3 * pout)270 HPt3TransPt3( Transform3 T, HPoint3 *pin, Point3 *pout )
271 {
272   HPoint3 tp;
273 
274   HPt3Transform( T, pin, &tp );
275   if(tp.w != 1.0 && tp.w != 0.0) {
276     pout->x = tp.x / tp.w;
277     pout->y = tp.y / tp.w;
278     pout->z = tp.z / tp.w;
279   } else {
280     pout->x = tp.x;
281     pout->y = tp.y;
282     pout->z = tp.z;
283   }
284   return tp.w;
285 }
286 
287 /*
288  * Pt3ToHPt4: convert 3-vectors to 4-vectors by padding with 1.0 's.
289  *
290  * Charlie Gunn
291  * Nov 26, 1991: originally written
292  */
293 static inline void
Pt3ToHPt3(Point3 * v3,HPoint3 * v4,int n)294 Pt3ToHPt3(Point3 *v3, HPoint3 *v4, int n)
295 {
296   int i;
297   for (i = 0; i < n; ++i) {
298     v4[i].x = v3[i].x;
299     v4[i].y = v3[i].y;
300     v4[i].z = v3[i].z;
301     v4[i].w = 1.0;
302   }
303 }
304 
305 static inline void
HPt3ToPt3(HPoint3 * hp,Point3 * p)306 HPt3ToPt3( HPoint3 *hp, Point3 *p )
307 {
308   if(hp->w == 1.0 || hp->w == 0.0) {
309     memcpy(p, hp, sizeof(Point3));
310   } else {
311     p->x = hp->x / hp->w;
312     p->y = hp->y / hp->w;
313     p->z = hp->z / hp->w;
314   }
315 }
316 
317 /* Transform a 4-point to a 3-point according to the mapping defined
318  * in "axes"
319  */
320 static inline void
Pt4ToHPt3(HPoint3 * pt4,int * axes,HPoint3 * hp3)321 Pt4ToHPt3(HPoint3 *pt4, int *axes, HPoint3 *hp3)
322 {
323   HPt3Coord *from = (HPt3Coord *)pt4, *to = (HPt3Coord *)hp3;
324   HPoint3 tmp;
325   int i;
326 
327   if (!axes) {
328     if (pt4 != hp3) {
329       hp3->x = pt4->x;
330       hp3->y = pt4->y;
331       hp3->z = pt4->z;
332     }
333     hp3->w = 1.0;
334     return;
335   }
336 
337   if (pt4 == hp3) {
338     tmp = *pt4;
339     from = (HPt3Coord *)&tmp;
340   }
341 
342   for (i = 0; i < 3; i++) {
343     if (axes[i] > 3) {
344       to[i] = 0.0;
345     } else if (axes[i] != -1) {
346       to[i] = from[axes[i]];
347     }
348   }
349   hp3->w = 1.0;
350 }
351 
352 /* also need an in-place dehomogenization routine which
353    sets the w-coordinate to 1.0, unless the original one is zero */
354 static inline void
HPt3Dehomogenize(HPoint3 * hp1,HPoint3 * hp2)355 HPt3Dehomogenize(HPoint3 *hp1, HPoint3 *hp2)
356 {
357   HPt3Coord inv;
358   if (hp1->w == 1.0 || hp1->w == 0.0) {
359     if (true || hp2 != hp1) *hp2 = *hp1;
360     return;
361   }
362   /* else if ( || hp->w == 0.0) hp->w = .000001;*/
363   inv = 1.0 / hp1->w;
364   hp2->x = hp1->x * inv;
365   hp2->y = hp1->y * inv;
366   hp2->z = hp1->z * inv;
367   hp2->w = 1.0;
368 }
369 
370 /* Fishy procedure */
371 static inline void
HPt3Dual(HPoint3 * pt,HPlane3 * pl)372 HPt3Dual( HPoint3 *pt, HPlane3 *pl )
373 {
374   pl->a = pt->x;
375   pl->b = pt->y;
376   pl->c = pt->z;
377   pl->d = pt->w;
378 }
379 
380 static inline void
HPt3LinSum(HPt3Coord scale1,HPoint3 * in1,HPt3Coord scale2,HPoint3 * in2,HPoint3 * out)381 HPt3LinSum (HPt3Coord scale1, HPoint3 *in1, HPt3Coord scale2, HPoint3 *in2, HPoint3 *out)
382 {
383   if ((in1->w == 0) || (in2->w == 0)) {
384     out->w = 0;
385     out->x = scale1 * in1->x + scale2 * in2->x;
386     out->y = scale1 * in1->y + scale2 * in2->y;
387     out->z = scale1 * in1->z + scale2 * in2->z;
388     return;
389   }
390   out->w = 1;
391   out->x = scale1 * (in1->x/in1->w) + scale2 * (in2->x/in2->w);
392   out->y = scale1 * (in1->y/in1->w) + scale2 * (in2->y/in2->w);
393   out->z = scale1 * (in1->z/in1->w) + scale2 * (in2->z/in2->w);
394 }
395 
396 static inline void
HPt3LinSumDenorm(HPt3Coord scale1,HPoint3 * in1,HPt3Coord scale2,HPoint3 * in2,HPoint3 * out)397 HPt3LinSumDenorm(HPt3Coord scale1, HPoint3 *in1, HPt3Coord scale2, HPoint3 *in2, HPoint3 *out)
398 {
399   if ((in1->w == 0) || (in2->w == 0)) {
400     out->w = 0;
401     out->x = scale1 * in1->x + scale2 * in2->x;
402     out->y = scale1 * in1->y + scale2 * in2->y;
403     out->z = scale1 * in1->z + scale2 * in2->z;
404     return;
405   }
406   out->w = scale1 * in1->w + scale2 * in2->w;
407   scale1 = scale1 * out->w / in1->w;
408   scale2 = scale2 * out->w / in2->w;
409   out->x = scale1 * in1->x + scale2 * in2->x;
410   out->y = scale1 * in1->y + scale2 * in2->y;
411   out->z = scale1 * in1->z + scale2 * in2->z;
412 }
413 
414 static inline void
HPt3SizeOne(HPoint3 * pt,HPoint3 * out)415 HPt3SizeOne ( HPoint3 *pt, HPoint3 *out )
416 {
417   HPt3Coord size;
418 
419   size = sqrt (pt->x * pt->x + pt->y * pt->y + pt->z * pt->z);
420   if (size == 0) return;
421   out->x = pt->x / size;
422   out->y = pt->y / size;
423   out->z = pt->z / size;
424   out->w = 1.;
425 }
426 
427 /* inner product of R4 */
428 static inline HPt3Coord
HPt3R40Dot(HPoint3 * a,HPoint3 * b)429 HPt3R40Dot(HPoint3 *a, HPoint3 *b)
430 {
431   return a->x*b->x + a->y*b->y + a->z*b->z + a->w*b->w;
432 }
433 
434 /* inner product of R(3,1) */
435 static inline HPt3Coord
HPt3R31Dot(HPoint3 * a,HPoint3 * b)436 HPt3R31Dot(HPoint3 *a, HPoint3 *b)
437 {
438   return a->x*b->x + a->y*b->y + a->z*b->z - a->w*b->w;
439 }
440 
441 /* inner product of R(3,0) */
442 static inline HPt3Coord
HPt3R30Dot(HPoint3 * a,HPoint3 * b)443 HPt3R30Dot(HPoint3 *a, HPoint3 *b)
444 {
445   double w2 = a->w*b->w;
446   if (w2 == 1.0 || w2 == 0.0)
447     return a->x*b->x + a->y*b->y + a->z*b->z;
448   else
449     return (a->x*b->x + a->y*b->y + a->z*b->z) / w2;
450 }
451 
452 static inline HPt3Coord
HPt3SpaceDot(HPoint3 * a,HPoint3 * b,int space)453 HPt3SpaceDot(HPoint3 *a, HPoint3 *b, int space)
454 {
455   switch (space) {
456   case TM_EUCLIDEAN:
457   default:
458     return HPt3R30Dot(a,b);
459     break;
460   case TM_HYPERBOLIC:
461     return HPt3R31Dot(a,b);
462     break;
463   case TM_SPHERICAL:
464     return HPt3R40Dot(a,b);
465     break;
466   }
467 }
468 
469 static inline HPt3Coord
HPt3DotPt3(HPoint3 * a,Point3 * b)470 HPt3DotPt3(HPoint3 *a, Point3 *b)
471 {
472   HPt3Coord scp;
473 
474   scp = a->x*b->x + a->y*b->y + a->z*b->z;
475   if (a->w != 0 && a->w != 1.0) {
476     return scp / a->w;
477   } else {
478     return scp;
479   }
480 }
481 
482 /* normalize a point a in R4 so that <a,a> = 1 */
483 static inline void
HPt3R40Normalize(HPoint3 * a)484 HPt3R40Normalize(HPoint3 *a)
485 {
486   float len = sqrt( (double)(HPt3R40Dot(a,a)) );
487   if (len > 0) {
488     len = 1 / len;
489     a->x *= len;
490     a->y *= len;
491     a->z *= len;
492     a->w *= len;
493   }
494 }
495 
496 /* normalize a point a in R(3,1) so that <a,a> = sign(<a,a>) */
497 static inline void
HPt3R31Normalize(HPoint3 * a)498 HPt3R31Normalize(HPoint3 *a)
499 {
500   float len = sqrt( fabs( (double)(HPt3R31Dot(a,a)) ) );
501   if (len > 0) {
502     len = 1 / len;
503     a->x *= len;
504     a->y *= len;
505     a->z *= len;
506     a->w *= len;
507   }
508 }
509 
510 /* normalize a point a in R(3,0) so that <a,a> = 1 */
511 static inline void
HPt3R30Normalize(HPoint3 * a)512 HPt3R30Normalize(HPoint3 *a)
513 {
514   float len = sqrt( (double)(HPt3R30Dot(a,a)) );
515   if (len > 0) {
516     len = 1 / len;
517     a->x *= len;
518     a->y *= len;
519     a->z *= len;
520   }
521 }
522 
523 static inline void
HPt3SpaceNormalize(HPoint3 * a,int space)524 HPt3SpaceNormalize(HPoint3 *a, int space)
525 {
526   switch (space) {
527   case TM_EUCLIDEAN:
528   default:
529     HPt3R30Normalize(a);
530     break;
531   case TM_HYPERBOLIC:
532     HPt3R31Normalize(a);
533     break;
534   case TM_SPHERICAL:
535     HPt3R40Normalize(a);
536     break;
537   }
538 }
539 
540 static inline HPt3Coord
HPt3HypDistance(HPoint3 * a,HPoint3 * b)541 HPt3HypDistance(HPoint3 *a, HPoint3 *b)
542 {
543   float aa, bb, ab;
544   aa = HPt3R31Dot(a,a);
545   bb = HPt3R31Dot(b,b);
546   ab = HPt3R31Dot(a,b);
547   return acosh( fabs(ab / sqrt( aa * bb ) ));
548 }
549 
550 static inline HPt3Coord
HPt3Distance(HPoint3 * a,HPoint3 * b)551 HPt3Distance(HPoint3 *a, HPoint3 *b)
552 {
553   float dx, dy, dz;
554   float w1w2;
555 
556   w1w2 = a->w * b->w;
557   if( w1w2 == 0. )
558     return 0.;
559 
560   dx = b->w * a->x - b->x * a->w;
561   dy = b->w * a->y - b->y * a->w;
562   dz = b->w * a->z - b->z * a->w;
563 
564   return (sqrt( dx*dx + dy*dy + dz*dz )) / w1w2;
565 }
566 
567 static inline HPt3Coord
HPt3SphDistance(HPoint3 * a,HPoint3 * b)568 HPt3SphDistance(HPoint3 *a, HPoint3 *b)
569 {
570   return acos( HPt3R40Dot(a,b) / sqrt( HPt3R40Dot(a,a) * HPt3R40Dot(b,b) ) );
571 }
572 
573 static inline HPt3Coord
HPt3SpaceDistance(HPoint3 * a,HPoint3 * b,int space)574 HPt3SpaceDistance(HPoint3 *a, HPoint3 *b, int space)
575 {
576   switch (space) {
577   case TM_EUCLIDEAN:
578   default:
579     return HPt3Distance(a,b);
580     break;
581   case TM_HYPERBOLIC:
582     return HPt3HypDistance(a,b);
583     break;
584   case TM_SPHERICAL:
585     return HPt3SphDistance(a,b);
586     break;
587   }
588 }
589 
590 static inline void
HPt3Sub(HPoint3 * a,HPoint3 * b,HPoint3 * aminusb)591 HPt3Sub(HPoint3 *a, HPoint3 *b, HPoint3 *aminusb)
592 {
593   aminusb->x = a->x - b->x;
594   aminusb->y = a->y - b->y;
595   aminusb->z = a->z - b->z;
596   aminusb->w = a->w - b->w;
597 }
598 
599 static inline void
HPt3Scale(HPt3Coord s,HPoint3 * a,HPoint3 * sa)600 HPt3Scale(HPt3Coord s, HPoint3 *a, HPoint3 *sa)
601 {
602   sa->x = s * a->x;
603   sa->y = s * a->y;
604   sa->z = s * a->z;
605   sa->w = s * a->w;
606 }
607 
608 static inline void
HPt3GramSchmidt(HPoint3 * base,HPoint3 * v)609 HPt3GramSchmidt(HPoint3 *base, HPoint3 *v)
610 {
611   HPt3SpaceGramSchmidt(base, v, TM_EUCLIDEAN);
612 }
613 
614 static inline void
HPt3HypGramSchmidt(HPoint3 * base,HPoint3 * v)615 HPt3HypGramSchmidt(HPoint3 *base, HPoint3 *v)
616 {
617   HPt3SpaceGramSchmidt(base, v, TM_HYPERBOLIC);
618 }
619 
620 static inline void
HPt3SphGramSchmidt(HPoint3 * base,HPoint3 * v)621 HPt3SphGramSchmidt(HPoint3 *base, HPoint3 *v)
622 {
623   HPt3SpaceGramSchmidt(base, v, TM_SPHERICAL);
624 }
625 
626 /* Modifies v to arrange that <base,v> = 0,
627    i.e. v is a tangent vector based at base */
628 static inline void
HPt3SpaceGramSchmidt(HPoint3 * base,HPoint3 * v,int space)629 HPt3SpaceGramSchmidt(HPoint3 *base, HPoint3 *v, int space)
630 {
631   HPt3Coord d,e;
632   HPoint3 tmp;
633 
634   d = HPt3SpaceDot(base,v,space);
635   e = HPt3SpaceDot(base,base,space);
636   if (e == 0.0) {
637     fprintf(stderr, "GramSchmidt: invalid base point.\n");
638     e = 1.0;
639   }
640   HPt3Scale((HPt3Coord)(d / e), base, &tmp);
641   HPt3Sub(v, &tmp, v);
642 }
643 
644 #if 0
645 
646 /* This stuff doesn't compile yet; in progress: */
647 
648 static inline HPt3Coord
649 HPt3Angle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2)
650 {
651   /*
652    * I'm not sure that this will work in the euclidean case!!! mbp
653    */
654   return HPt3SpaceAngle(base, v1, v2, TM_EUCLIDEAN);
655 }
656 
657 static inline HPt3Coord
658 HPt3HypAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2)
659 {
660   return HPt3SpaceAngle(base, v1, v2, TM_HYPERBOLIC);
661 }
662 
663 static inline HPt3Coord
664 HPt3SphAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2)
665 {
666   return HPt3SpaceAngle(base, v1, v2, TM_SPHERICAL);
667 }
668 
669 static inline HPt3Coord
670 HPt3SpaceAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2, int space)
671 {
672   double d, n;
673   HPoint3 v1n = *v1;
674   HPoint3 v2n = *v2;
675   HPt3SpaceGramSchmidt(base, &v1n, space);
676   HPt3SpaceGramSchmidt(base, &v2n, space);
677   d = HPt3SpaceDot(&v1n,&v1n,space) * HPt3SpaceDot(&v2n,&v2n,space);
678   if (d <= 0.0) {
679     fprintf(stderr,"HPt3SpaceAngle: invalid denominator\n");
680     return 0.0;
681   }
682   n = HPt3SpaceDot(&v1n, &v2n, space);
683   return acos( n / sqrt(d) );
684 }
685 
686 #endif
687 
688 static inline void
HPt3SubPt3(HPoint3 * p1,HPoint3 * p2,Point3 * v)689 HPt3SubPt3(HPoint3 *p1, HPoint3 *p2, Point3 *v)
690 {
691   if (p1->w == p2->w) {
692     v->x = p1->x - p2->x; v->y = p1->y - p2->y; v->z = p1->z - p2->z;
693   } else if (p1->w == 0) {
694     *v = *(Point3 *)p1;
695     return;
696   } else if (p2->w == 0) {
697     *v = *HPoint3Point3(p2);
698     Pt3Mul(-1.0, v, v);
699     return;
700   } else {
701     HPt3Coord s = p2->w / p1->w;
702     v->x = p1->x*s - p2->x; v->y = p1->y*s - p2->y; v->z = p1->z*s - p2->z;
703   }
704   if(p2->w != 1.0 && p2->w != 0.0)
705     v->x /= p2->w, v->y /= p2->w, v->z /= p2->w;
706 }
707 
708 /* assume that min and max are  dehomogenized */
HPt3MinMax(HPoint3 * min,HPoint3 * max,HPoint3 * other)709 static inline void HPt3MinMax(HPoint3 *min, HPoint3 *max, HPoint3 *other)
710 {
711   HPt3Coord oc = other->w;
712 
713   if (oc == 0.0) {
714     oc = 1.0;
715   }
716   if(oc * min->x > other->x) min->x = other->x / oc;
717   else if(oc * max->x < other->x) max->x = other->x / oc;
718   if(oc * min->y > other->y) min->y = other->y / oc;
719   else if(oc * max->y < other->y) max->y = other->y / oc;
720   if(oc * min->z > other->z) min->z = other->z / oc;
721   else if(oc * max->z < other->z) max->z = other->z / oc;
722 }
723 
Pt3MinMax(HPoint3 * min,HPoint3 * max,HPoint3 * other)724 static inline void Pt3MinMax(HPoint3 *min, HPoint3 *max, HPoint3 *other)
725 {
726   if(min->x > other->x) min->x = other->x;
727   else if(max->x < other->x) max->x = other->x;
728   if(min->y > other->y) min->y = other->y;
729   else if(max->y < other->y) max->y = other->y;
730   if(min->z > other->z) min->z = other->z;
731   else if(max->z < other->z) max->z = other->z;
732 }
733 
Pt4MinMax(HPoint3 * min,HPoint3 * max,HPoint3 * other)734 static inline void Pt4MinMax(HPoint3 *min, HPoint3 *max, HPoint3 *other)
735 {
736   if(min->x > other->x) min->x = other->x;
737   else if(max->x < other->x) max->x = other->x;
738   if(min->y > other->y) min->y = other->y;
739   else if(max->y < other->y) max->y = other->y;
740   if(min->z > other->z) min->z = other->z;
741   else if(max->z < other->z) max->z = other->z;
742   if(min->w > other->w) min->w = other->w;
743   else if(max->w < other->w) max->w = other->w;
744 }
745 
746 #endif /* _GV_HPOINT3_H_ */
747 
748 /*
749  * Local Variables: ***
750  * c-basic-offset: 2 ***
751  * End: ***
752  */
753