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