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