1 /*
2 3D vector routines.
3
4 clib v1.1
5
6 Copyright (C) 1997-1998 Per Kraulis
7 27-Dec-1996 first attempts
8 6-Feb-1997 fairly finished
9 10-Mar-1997 fixed bug in cross product routine
10 19-Feb-1998 minor mod's
11 19-Mar-1998 modified for hgen
12 */
13
14 #include "vector3.h"
15
16 /* public ====================
17 #include <boolean.h>
18
19 typedef struct {
20 double x, y, z;
21 } vector3;
22 ==================== public */
23
24 #include <assert.h>
25 #include <math.h>
26
27
28 /*------------------------------------------------------------*/
29 void
v3_initialize(vector3 * dest,const double x,const double y,const double z)30 v3_initialize (vector3 *dest, const double x, const double y, const double z)
31 {
32 /* pre */
33 assert (dest);
34
35 dest->x = x;
36 dest->y = y;
37 dest->z = z;
38 }
39
40
41 /*------------------------------------------------------------*/
42 void
v3_sum(vector3 * dest,const vector3 * v1,const vector3 * v2)43 v3_sum (vector3 *dest, const vector3 *v1, const vector3 *v2)
44 /*
45 dest = v1 + v2
46 */
47 {
48 /* pre */
49 assert (dest);
50 assert (v1);
51 assert (v2);
52
53 dest->x = v1->x + v2->x;
54 dest->y = v1->y + v2->y;
55 dest->z = v1->z + v2->z;
56 }
57
58
59 /*------------------------------------------------------------*/
60 void
v3_difference(vector3 * dest,const vector3 * v1,const vector3 * v2)61 v3_difference (vector3 *dest, const vector3 *v1, const vector3 *v2)
62 /*
63 dest = v1 - v2
64 */
65 {
66 /* pre */
67 assert (dest);
68 assert (v1);
69 assert (v2);
70
71 dest->x = v1->x - v2->x;
72 dest->y = v1->y - v2->y;
73 dest->z = v1->z - v2->z;
74 }
75
76
77 /*------------------------------------------------------------*/
78 void
v3_scaled(vector3 * dest,const double s,const vector3 * src)79 v3_scaled (vector3 *dest, const double s, const vector3 *src)
80 /*
81 dest = s * src
82 */
83 {
84 /* pre */
85 assert (dest);
86 assert (src);
87
88 dest->x = s * src->x;
89 dest->y = s * src->y;
90 dest->z = s * src->z;
91 }
92
93
94 /*------------------------------------------------------------*/
95 void
v3_add(vector3 * dest,const vector3 * v)96 v3_add (vector3 *dest, const vector3 *v)
97 /*
98 dest += v
99 */
100 {
101 /* pre */
102 assert (dest);
103 assert (v);
104
105 dest->x += v->x;
106 dest->y += v->y;
107 dest->z += v->z;
108 }
109
110
111 /*------------------------------------------------------------*/
112 void
v3_subtract(vector3 * dest,const vector3 * v)113 v3_subtract (vector3 *dest, const vector3 *v)
114 /*
115 dest -= v
116 */
117 {
118 /* pre */
119 assert (dest);
120 assert (v);
121
122 dest->x -= v->x;
123 dest->y -= v->y;
124 dest->z -= v->z;
125 }
126
127
128 /*------------------------------------------------------------*/
129 void
v3_scale(vector3 * dest,const double s)130 v3_scale (vector3 *dest, const double s)
131 /*
132 dest *= s
133 */
134 {
135 /* pre */
136 assert (dest);
137
138 dest->x *= s;
139 dest->y *= s;
140 dest->z *= s;
141 }
142
143
144 /*------------------------------------------------------------*/
145 void
v3_invert(vector3 * v)146 v3_invert (vector3 *v)
147 /*
148 v = (1/x, 1/y, 1/z)
149 */
150 {
151 /* pre */
152 assert (v);
153 assert (v->x != 0.0);
154 assert (v->y != 0.0);
155 assert (v->z != 0.0);
156
157 v->x = 1.0 / v->x;
158 v->y = 1.0 / v->y;
159 v->z = 1.0 / v->z;
160 }
161
162
163 /*------------------------------------------------------------*/
164 void
v3_reverse(vector3 * v)165 v3_reverse (vector3 *v)
166 /*
167 v = -v
168 */
169 {
170 /* pre */
171 assert (v);
172
173 v->x = - v->x;
174 v->y = - v->y;
175 v->z = - v->z;
176 }
177
178
179 /*------------------------------------------------------------*/
180 void
v3_sum_scaled(vector3 * dest,const vector3 * v1,const double s,const vector3 * v2)181 v3_sum_scaled (vector3 *dest, const vector3 *v1,
182 const double s, const vector3 *v2)
183 /*
184 dest = v1 + s * v2
185 */
186 {
187 /* pre */
188 assert (dest);
189 assert (v1);
190 assert (v2);
191
192 dest->x = v1->x + s * v2->x;
193 dest->y = v1->y + s * v2->y;
194 dest->z = v1->z + s * v2->z;
195 }
196
197
198 /*------------------------------------------------------------*/
199 void
v3_add_scaled(vector3 * dest,const double s,const vector3 * v)200 v3_add_scaled (vector3 *dest, const double s, const vector3 *v)
201 /*
202 dest += s * v
203 */
204 {
205 /* pre */
206 assert (dest);
207 assert (v);
208
209 dest->x += s * v->x;
210 dest->y += s * v->y;
211 dest->z += s * v->z;
212 }
213
214
215 /*------------------------------------------------------------*/
216 double
v3_length(const vector3 * v)217 v3_length (const vector3 *v)
218 {
219 /* pre */
220 assert (v);
221
222 return sqrt (v->x * v->x + v->y * v->y + v->z * v->z);
223 }
224
225
226 /*------------------------------------------------------------*/
227 double
v3_angle(const vector3 * v1,const vector3 * v2)228 v3_angle (const vector3 *v1, const vector3 *v2)
229 /*
230 The angle (in radians) between the vectors v1 and v2.
231 */
232 {
233 /* pre */
234 assert (v1);
235 assert (v2);
236 assert (v3_length (v1) > 0.0);
237 assert (v3_length (v2) > 0.0);
238
239 return acos ((v1->x * v2->x + v1->y * v2->y + v1->z * v2->z) /
240 sqrt ((v1->x * v1->x + v1->y * v1->y + v1->z * v1->z) *
241 (v2->x * v2->x + v2->y * v2->y + v2->z * v2->z)));
242 }
243
244
245 /*------------------------------------------------------------*/
246 double
v3_angle_points(const vector3 * p1,const vector3 * p2,const vector3 * p3)247 v3_angle_points (const vector3 *p1, const vector3 *p2, const vector3 *p3)
248 /*
249 The angle (in radians) formed between the vector from p2 to p1
250 and the vector from p2 to p3.
251 */
252 {
253 vector3 v1, v2;
254
255 /* pre */
256 assert (p1);
257 assert (p2);
258 assert (p3);
259
260 v3_difference (&v1, p1, p2);
261 v3_difference (&v2, p3, p2);
262 return v3_angle (&v1, &v2);
263 }
264
265
266 /*------------------------------------------------------------*/
267 double
v3_torsion(const vector3 * p1,const vector3 * v1,const vector3 * p2,const vector3 * v2)268 v3_torsion (const vector3 *p1, const vector3 *v1,
269 const vector3 *p2, const vector3 *v2)
270 /*
271 The torsion angle (in radians) between the vectors v1 and v2
272 when viewed along the vector from p1 to p2.
273 */
274 {
275 double angle;
276 vector3 dir, x1, x2;
277
278 /* pre */
279 assert (p1);
280 assert (v1);
281 assert (p2);
282 assert (v2);
283
284 v3_difference (&dir, p2, p1);
285 v3_cross_product (&x1, &dir, v1);
286 v3_cross_product (&x2, &dir, v2);
287 angle = v3_angle (&x1, &x2);
288 return (v3_dot_product (v1, &x2) > 0.0) ? angle : -angle;
289 }
290
291
292 /*------------------------------------------------------------*/
293 double
v3_torsion_points(const vector3 * p1,const vector3 * p2,const vector3 * p3,const vector3 * p4)294 v3_torsion_points (const vector3 *p1, const vector3 *p2,
295 const vector3 *p3, const vector3 *p4)
296 /*
297 The torsion angle (in radians) formed between the points.
298 */
299 {
300 vector3 v1, v2;
301
302 /* pre */
303 assert (p1);
304 assert (p2);
305 assert (p3);
306 assert (p4);
307 assert (p1 != p2);
308 assert (p2 != p3);
309 assert (p3 != p4);
310
311 v3_difference (&v1, p1, p2);
312 v3_difference (&v2, p4, p3);
313 return v3_torsion (p2, &v1, p3, &v2);
314 }
315
316
317 /*------------------------------------------------------------*/
318 double
v3_dot_product(const vector3 * v1,const vector3 * v2)319 v3_dot_product (const vector3 *v1, const vector3 *v2)
320 {
321 /* pre */
322 assert (v1);
323 assert (v2);
324
325 return v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
326 }
327
328
329 /*------------------------------------------------------------*/
330 void
v3_cross_product(vector3 * vz,const vector3 * vx,const vector3 * vy)331 v3_cross_product (vector3 *vz, const vector3 *vx, const vector3 *vy)
332 {
333 /* pre */
334 assert (vz);
335 assert (vx);
336 assert (vy);
337 assert (vz != vx);
338 assert (vz != vy);
339
340 vz->x = vx->y * vy->z - vx->z * vy->y;
341 vz->y = vx->z * vy->x - vx->x * vy->z;
342 vz->z = vx->x * vy->y - vx->y * vy->x;
343 }
344
345
346 /*------------------------------------------------------------*/
347 void
v3_triangle_normal(vector3 * n,const vector3 * p1,const vector3 * p2,const vector3 * p3)348 v3_triangle_normal (vector3 *n,
349 const vector3 *p1, const vector3 *p2, const vector3 *p3)
350 /*
351 The normal vector n for the plane defined by the three points.
352 */
353 {
354 vector3 p12, p23;
355
356 /* pre */
357 assert (n);
358 assert (p1);
359 assert (p2);
360 assert (p3);
361 assert (v3_distance (p1, p2) > 0.0);
362 assert (v3_distance (p2, p3) > 0.0);
363 assert (v3_distance (p3, p1) > 0.0);
364
365 v3_difference (&p12, p2, p1);
366 v3_difference (&p23, p3, p2);
367 v3_cross_product (n, &p12, &p23);
368 v3_normalize (n);
369 }
370
371
372 /*------------------------------------------------------------*/
373 double
v3_sqdistance(const vector3 * p1,const vector3 * p2)374 v3_sqdistance (const vector3 *p1, const vector3 *p2)
375 /*
376 The squared distance between the points.
377 */
378 {
379 register double xdiff, ydiff, zdiff;
380
381 /* pre */
382 assert (p1);
383 assert (p2);
384
385 xdiff = p1->x - p2->x;
386 ydiff = p1->y - p2->y;
387 zdiff = p1->z - p2->z;
388
389 return xdiff * xdiff + ydiff * ydiff + zdiff * zdiff;
390 }
391
392
393 /*------------------------------------------------------------*/
394 double
v3_distance(const vector3 * p1,const vector3 * p2)395 v3_distance (const vector3 *p1, const vector3 *p2)
396 {
397 register double xdiff, ydiff, zdiff;
398
399 /* pre */
400 assert (p1);
401 assert (p2);
402
403 xdiff = p1->x - p2->x;
404 ydiff = p1->y - p2->y;
405 zdiff = p1->z - p2->z;
406
407 return sqrt (xdiff * xdiff + ydiff * ydiff + zdiff * zdiff);
408 }
409
410
411 /*------------------------------------------------------------*/
412 boolean
v3_close(const vector3 * v1,const vector3 * v2,const double sqdistance)413 v3_close (const vector3 *v1, const vector3 *v2, const double sqdistance)
414 /*
415 Are the two points closer than the given squared distance?
416 */
417 {
418 register double xdiff;
419
420 /* pre */
421 assert (v1);
422 assert (v2);
423
424 xdiff = v1->x - v2->x;
425 xdiff *= xdiff;
426 if (xdiff > sqdistance) {
427 return FALSE;
428 } else {
429 register double ydiff = v1->y - v2->y;
430 ydiff *= ydiff;
431 if (ydiff > sqdistance) {
432 return FALSE;
433 } else {
434 register double zdiff = v1->z - v2->z;
435 return (xdiff + ydiff + zdiff * zdiff) <= sqdistance;
436 }
437 }
438 }
439
440
441 /*------------------------------------------------------------*/
442 void
v3_normalize(vector3 * v)443 v3_normalize (vector3 *v)
444 /*
445 v /= length (v)
446 */
447 {
448 register double ir;
449
450 /* pre */
451 assert (v);
452 assert (v3_length (v) > 0.0);
453
454 ir = 1.0 / sqrt (v->x * v->x + v->y * v->y + v->z * v->z);
455 v->x *= ir;
456 v->y *= ir;
457 v->z *= ir;
458 }
459
460
461 /*------------------------------------------------------------*/
462 void
v3_middle(vector3 * dest,const vector3 * p1,const vector3 * p2)463 v3_middle (vector3 *dest, const vector3 *p1, const vector3 *p2)
464 /*
465 dest = (p1 + p2) / 2
466 */
467 {
468 /* pre */
469 assert (dest);
470 assert (p1);
471 assert (p2);
472
473 dest->x = 0.5 * (p1->x + p2->x);
474 dest->y = 0.5 * (p1->y + p2->y);
475 dest->z = 0.5 * (p1->z + p2->z);
476 }
477
478
479 /*------------------------------------------------------------*/
480 void
v3_between(vector3 * dest,const vector3 * p1,const vector3 * p2,const double fraction)481 v3_between (vector3 *dest,
482 const vector3 *p1, const vector3 *p2, const double fraction)
483 /*
484 dest = fraction * (p2 - p1) + p1
485 */
486 {
487 /* pre */
488 assert (dest);
489 assert (p1);
490 assert (p2);
491
492 dest->x = fraction * (p2->x - p1->x) + p1->x;
493 dest->y = fraction * (p2->y - p1->y) + p1->y;
494 dest->z = fraction * (p2->z - p1->z) + p1->z;
495 }
496
497
498 /*------------------------------------------------------------*/
499 int
v3_different(const vector3 * v1,const vector3 * v2)500 v3_different (const vector3 *v1, const vector3 *v2)
501 {
502 /* pre */
503 assert (v1);
504 assert (v2);
505
506 return ((v1->x != v2->x) || (v1->y != v2->y) || (v1->z != v2->z));
507 }
508