1 /*
2 2d and 3d Vector C Library
3 by Andrew Glassner
4 from "Graphics Gems", Academic Press, 1990
5 */
6
7 /*
8 * EULA: The Graphics Gems code is copyright-protected. In other words,
9 * you cannot claim the text of the code as your own and resell it.
10 * Using the code is permitted in any program, product, or library,
11 * non-commercial or commercial. Giving credit is not required, though
12 * is a nice gesture. The code comes as-is, and if there are any flaws
13 * or problems with any Gems code, nobody involved with Gems - authors,
14 * editors, publishers, or webmasters - are to be held responsible.
15 * Basically, don't be a jerk, and remember that anything free comes
16 * with no guarantee.
17 */
18
19 #include <math.h>
20 #include <stdlib.h>
21 #include "GraphicsGems.h"
22
23 /******************/
24 /* 2d Library */
25 /******************/
26
27 /* returns squared length of input vector */
28 double V2SquaredLength(a)
29 Vector2 *a;
30 { return((a->x * a->x)+(a->y * a->y));
31 }
32
33 /* returns length of input vector */
34 double V2Length(a)
35 Vector2 *a;
36 {
37 return(sqrt(V2SquaredLength(a)));
38 }
39
40 /* negates the input vector and returns it */
41 Vector2 *V2Negate(v)
42 Vector2 *v;
43 {
44 v->x = -v->x; v->y = -v->y;
45 return(v);
46 }
47
48 /* normalizes the input vector and returns it */
49 Vector2 *V2Normalize(v)
50 Vector2 *v;
51 {
52 double len = V2Length(v);
53 if (len != 0.0) { v->x /= len; v->y /= len; }
54 return(v);
55 }
56
57
58 /* scales the input vector to the new length and returns it */
59 Vector2 *V2Scale(v, newlen)
60 Vector2 *v;
61 double newlen;
62 {
63 double len = V2Length(v);
64 if (len != 0.0) { v->x *= newlen/len; v->y *= newlen/len; }
65 return(v);
66 }
67
68 /* return vector sum c = a+b */
69 Vector2 *V2Add(a, b, c)
70 Vector2 *a, *b, *c;
71 {
72 c->x = a->x+b->x; c->y = a->y+b->y;
73 return(c);
74 }
75
76 /* return vector difference c = a-b */
77 Vector2 *V2Sub(a, b, c)
78 Vector2 *a, *b, *c;
79 {
80 c->x = a->x-b->x; c->y = a->y-b->y;
81 return(c);
82 }
83
84 /* return the dot product of vectors a and b */
85 double V2Dot(a, b)
86 Vector2 *a, *b;
87 {
88 return((a->x*b->x)+(a->y*b->y));
89 }
90
91 /* linearly interpolate between vectors by an amount alpha */
92 /* and return the resulting vector. */
93 /* When alpha=0, result=lo. When alpha=1, result=hi. */
94 Vector2 *V2Lerp(lo, hi, alpha, result)
95 Vector2 *lo, *hi, *result;
96 double alpha;
97 {
98 result->x = LERP(alpha, lo->x, hi->x);
99 result->y = LERP(alpha, lo->y, hi->y);
100 return(result);
101 }
102
103
104 /* make a linear combination of two vectors and return the result. */
105 /* result = (a * ascl) + (b * bscl) */
106 Vector2 *V2Combine (a, b, result, ascl, bscl)
107 Vector2 *a, *b, *result;
108 double ascl, bscl;
109 {
110 result->x = (ascl * a->x) + (bscl * b->x);
111 result->y = (ascl * a->y) + (bscl * b->y);
112 return(result);
113 }
114
115 /* multiply two vectors together component-wise */
116 Vector2 *V2Mul (a, b, result)
117 Vector2 *a, *b, *result;
118 {
119 result->x = a->x * b->x;
120 result->y = a->y * b->y;
121 return(result);
122 }
123
124 /* return the distance between two points */
125 double V2DistanceBetween2Points(a, b)
126 Point2 *a, *b;
127 {
128 double dx = a->x - b->x;
129 double dy = a->y - b->y;
130 return(sqrt((dx*dx)+(dy*dy)));
131 }
132
133 /* return the vector perpendicular to the input vector a */
134 Vector2 *V2MakePerpendicular(a, ap)
135 Vector2 *a, *ap;
136 {
137 ap->x = -a->y;
138 ap->y = a->x;
139 return(ap);
140 }
141
142 /* create, initialize, and return a new vector */
143 Vector2 *V2New(x, y)
144 double x, y;
145 {
146 Vector2 *v = NEWTYPE(Vector2);
147 v->x = x; v->y = y;
148 return(v);
149 }
150
151
152 /* create, initialize, and return a duplicate vector */
153 Vector2 *V2Duplicate(a)
154 Vector2 *a;
155 {
156 Vector2 *v = NEWTYPE(Vector2);
157 v->x = a->x; v->y = a->y;
158 return(v);
159 }
160
161 /* multiply a point by a matrix and return the transformed point */
162 Point2 *V2MulPointByMatrix(p, m)
163 Point2 *p;
164 Matrix3 *m;
165 {
166 double w;
167 Point2 ptmp;
168 ptmp.x = (p->x * m->element[0][0]) +
169 (p->y * m->element[1][0]) + m->element[2][0];
170 ptmp.y = (p->x * m->element[0][1]) +
171 (p->y * m->element[1][1]) + m->element[2][1];
172 w = (p->x * m->element[0][2]) +
173 (p->y * m->element[1][2]) + m->element[2][2];
174 if (w != 0.0) { ptmp.x /= w; ptmp.y /= w; }
175 *p = ptmp;
176 return(p);
177 }
178
179 /* multiply together matrices c = ab */
180 /* note that c must not point to either of the input matrices */
181 Matrix3 *V2MatMul(a, b, c)
182 Matrix3 *a, *b, *c;
183 {
184 int i, j, k;
185 for (i=0; i<3; i++) {
186 for (j=0; j<3; j++) {
187 c->element[i][j] = 0;
188 for (k=0; k<3; k++) c->element[i][j] +=
189 a->element[i][k] * b->element[k][j];
190 }
191 }
192 return(c);
193 }
194
195
196
197
198 /******************/
199 /* 3d Library */
200 /******************/
201
202 /* returns squared length of input vector */
203 double V3SquaredLength(a)
204 Vector3 *a;
205 {
206 return((a->x * a->x)+(a->y * a->y)+(a->z * a->z));
207 }
208
209 /* returns length of input vector */
210 double V3Length(a)
211 Vector3 *a;
212 {
213 return(sqrt(V3SquaredLength(a)));
214 }
215
216 /* negates the input vector and returns it */
217 Vector3 *V3Negate(v)
218 Vector3 *v;
219 {
220 v->x = -v->x; v->y = -v->y; v->z = -v->z;
221 return(v);
222 }
223
224 /* normalizes the input vector and returns it */
225 Vector3 *V3Normalize(v)
226 Vector3 *v;
227 {
228 double len = V3Length(v);
229 if (len != 0.0) { v->x /= len; v->y /= len; v->z /= len; }
230 return(v);
231 }
232
233 /* scales the input vector to the new length and returns it */
234 Vector3 *V3Scale(v, newlen)
235 Vector3 *v;
236 double newlen;
237 {
238 double len = V3Length(v);
239 if (len != 0.0) {
240 v->x *= newlen/len; v->y *= newlen/len; v->z *= newlen/len;
241 }
242 return(v);
243 }
244
245
246 /* return vector sum c = a+b */
247 Vector3 *V3Add(a, b, c)
248 Vector3 *a, *b, *c;
249 {
250 c->x = a->x+b->x; c->y = a->y+b->y; c->z = a->z+b->z;
251 return(c);
252 }
253
254 /* return vector difference c = a-b */
255 Vector3 *V3Sub(a, b, c)
256 Vector3 *a, *b, *c;
257 {
258 c->x = a->x-b->x; c->y = a->y-b->y; c->z = a->z-b->z;
259 return(c);
260 }
261
262 /* return the dot product of vectors a and b */
263 double V3Dot(a, b)
264 Vector3 *a, *b;
265 {
266 return((a->x*b->x)+(a->y*b->y)+(a->z*b->z));
267 }
268
269 /* linearly interpolate between vectors by an amount alpha */
270 /* and return the resulting vector. */
271 /* When alpha=0, result=lo. When alpha=1, result=hi. */
272 Vector3 *V3Lerp(lo, hi, alpha, result)
273 Vector3 *lo, *hi, *result;
274 double alpha;
275 {
276 result->x = LERP(alpha, lo->x, hi->x);
277 result->y = LERP(alpha, lo->y, hi->y);
278 result->z = LERP(alpha, lo->z, hi->z);
279 return(result);
280 }
281
282 /* make a linear combination of two vectors and return the result. */
283 /* result = (a * ascl) + (b * bscl) */
284 Vector3 *V3Combine (a, b, result, ascl, bscl)
285 Vector3 *a, *b, *result;
286 double ascl, bscl;
287 {
288 result->x = (ascl * a->x) + (bscl * b->x);
289 result->y = (ascl * a->y) + (bscl * b->y);
290 result->z = (ascl * a->z) + (bscl * b->z);
291 return(result);
292 }
293
294
295 /* multiply two vectors together component-wise and return the result */
296 Vector3 *V3Mul (a, b, result)
297 Vector3 *a, *b, *result;
298 {
299 result->x = a->x * b->x;
300 result->y = a->y * b->y;
301 result->z = a->z * b->z;
302 return(result);
303 }
304
305 /* return the distance between two points */
306 double V3DistanceBetween2Points(a, b)
307 Point3 *a, *b;
308 {
309 double dx = a->x - b->x;
310 double dy = a->y - b->y;
311 double dz = a->z - b->z;
312 return(sqrt((dx*dx)+(dy*dy)+(dz*dz)));
313 }
314
315 /* return the cross product c = a cross b */
316 Vector3 *V3Cross(a, b, c)
317 Vector3 *a, *b, *c;
318 {
319 c->x = (a->y*b->z) - (a->z*b->y);
320 c->y = (a->z*b->x) - (a->x*b->z);
321 c->z = (a->x*b->y) - (a->y*b->x);
322 return(c);
323 }
324
325 /* create, initialize, and return a new vector */
326 Vector3 *V3New(x, y, z)
327 double x, y, z;
328 {
329 Vector3 *v = NEWTYPE(Vector3);
330 v->x = x; v->y = y; v->z = z;
331 return(v);
332 }
333
334 /* create, initialize, and return a duplicate vector */
335 Vector3 *V3Duplicate(a)
336 Vector3 *a;
337 {
338 Vector3 *v = NEWTYPE(Vector3);
339 v->x = a->x; v->y = a->y; v->z = a->z;
340 return(v);
341 }
342
343
344 /* multiply a point by a matrix and return the transformed point */
345 Point3 *V3MulPointByMatrix(p, m)
346 Point3 *p;
347 Matrix4 *m;
348 {
349 double w;
350 Point3 ptmp;
351 ptmp.x = (p->x * m->element[0][0]) + (p->y * m->element[1][0]) +
352 (p->z * m->element[2][0]) + m->element[3][0];
353 ptmp.y = (p->x * m->element[0][1]) + (p->y * m->element[1][1]) +
354 (p->z * m->element[2][1]) + m->element[3][1];
355 ptmp.z = (p->x * m->element[0][2]) + (p->y * m->element[1][2]) +
356 (p->z * m->element[2][2]) + m->element[3][2];
357 w = (p->x * m->element[0][3]) + (p->y * m->element[1][3]) +
358 (p->z * m->element[2][3]) + m->element[3][3];
359 if (w != 0.0) { ptmp.x /= w; ptmp.y /= w; ptmp.z /= w; }
360 *p = ptmp;
361 return(p);
362 }
363
364 /* multiply together matrices c = ab */
365 /* note that c must not point to either of the input matrices */
366 Matrix4 *V3MatMul(a, b, c)
367 Matrix4 *a, *b, *c;
368 {
369 int i, j, k;
370 for (i=0; i<4; i++) {
371 for (j=0; j<4; j++) {
372 c->element[i][j] = 0;
373 for (k=0; k<4; k++) c->element[i][j] +=
374 a->element[i][k] * b->element[k][j];
375 }
376 }
377 return(c);
378 }
379
380 /* binary greatest common divisor by Silver and Terzian. See Knuth */
381 /* both inputs must be >= 0 */
382 gcd(u, v)
383 int u, v;
384 {
385 int t, f;
386 if ((u<0) || (v<0)) return(1); /* error if u<0 or v<0 */
387 f = 1;
388 while ((0 == (u%2)) && (0 == (v%2))) {
389 u>>=1; v>>=1, f*=2;
390 }
391 if (u&01) { t = -v; goto B4; } else { t = u; }
392 B3: if (t > 0) { t >>= 1; } else { t = -((-t) >> 1); }
393 B4: if (0 == (t%2)) goto B3;
394
395 if (t > 0) u = t; else v = -t;
396 if (0 != (t = u - v)) goto B3;
397 return(u*f);
398 }
399
400 /***********************/
401 /* Useful Routines */
402 /***********************/
403
404 /* return roots of ax^2+bx+c */
405 /* stable algebra derived from Numerical Recipes by Press et al.*/
406 int quadraticRoots(a, b, c, roots)
407 double a, b, c, *roots;
408 {
409 double d, q;
410 int count = 0;
411 d = (b*b)-(4*a*c);
412 if (d < 0.0) { *roots = *(roots+1) = 0.0; return(0); }
413 q = -0.5 * (b + (SGN(b)*sqrt(d)));
414 if (a != 0.0) { *roots++ = q/a; count++; }
415 if (q != 0.0) { *roots++ = c/q; count++; }
416 return(count);
417 }
418
419
420 /* generic 1d regula-falsi step. f is function to evaluate */
421 /* interval known to contain root is given in left, right */
422 /* returns new estimate */
423 double RegulaFalsi(f, left, right)
424 double (*f)(), left, right;
425 {
426 double d = (*f)(right) - (*f)(left);
427 if (d != 0.0) return (right - (*f)(right)*(right-left)/d);
428 return((left+right)/2.0);
429 }
430
431 /* generic 1d Newton-Raphson step. f is function, df is derivative */
432 /* x is current best guess for root location. Returns new estimate */
433 double NewtonRaphson(f, df, x)
434 double (*f)(), (*df)(), x;
435 {
436 double d = (*df)(x);
437 if (d != 0.0) return (x-((*f)(x)/d));
438 return(x-1.0);
439 }
440
441
442 /* hybrid 1d Newton-Raphson/Regula Falsi root finder. */
443 /* input function f and its derivative df, an interval */
444 /* left, right known to contain the root, and an error tolerance */
445 /* Based on Blinn */
446 double findroot(left, right, tolerance, f, df)
447 double left, right, tolerance;
448 double (*f)(), (*df)();
449 {
450 double newx = left;
451 while (ABS((*f)(newx)) > tolerance) {
452 newx = NewtonRaphson(f, df, newx);
453 if (newx < left || newx > right)
454 newx = RegulaFalsi(f, left, right);
455 if ((*f)(newx) * (*f)(left) <= 0.0) right = newx;
456 else left = newx;
457 }
458 return(newx);
459 }
460