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