home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsii / ggveclib.c < prev    next >
C/C++ Source or Header  |  1993-10-06  |  11KB  |  470 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 projective matrix and return the transformed point */
  149. Point2 *V2MulPointByProjMatrix(pin, m, pout)
  150. Point2 *pin, *pout;
  151. Matrix3 *m;
  152. {
  153. double w;
  154.     pout->x = (pin->x * m->element[0][0]) + 
  155.              (pin->y * m->element[1][0]) + m->element[2][0];
  156.     pout->y = (pin->x * m->element[0][1]) + 
  157.              (pin->y * m->element[1][1]) + m->element[2][1];
  158.     w    = (pin->x * m->element[0][2]) + 
  159.              (pin->y * m->element[1][2]) + m->element[2][2];
  160.     if (w != 0.0) { pout->x /= w;  pout->y /= w; }
  161.     return(pout);
  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. /* transpose rotation portion of matrix a, return b */
  181. Matrix3 *TransposeMatrix3(a, b)
  182. Matrix3 *a, *b;
  183. {
  184. int i, j;
  185.     for (i=0; i<3; i++) {
  186.         for (j=0; j<3; j++)
  187.             b->element[i][j] = a->element[j][i];
  188.         }
  189.     return(b);
  190.     }
  191.  
  192.  
  193.  
  194.  
  195. /******************/
  196. /*   3d Library   */
  197. /******************/
  198.     
  199. /* returns squared length of input vector */    
  200. double V3SquaredLength(a) 
  201. Vector3 *a;
  202. {
  203.     return((a->x * a->x)+(a->y * a->y)+(a->z * a->z));
  204.     }
  205.  
  206. /* returns length of input vector */
  207. double V3Length(a) 
  208. Vector3 *a;
  209. {
  210.     return(sqrt(V3SquaredLength(a)));
  211.     }
  212.  
  213. /* negates the input vector and returns it */
  214. Vector3 *V3Negate(v) 
  215. Vector3 *v;
  216. {
  217.     v->x = -v->x;  v->y = -v->y;  v->z = -v->z;
  218.     return(v);
  219.     }
  220.  
  221. /* normalizes the input vector and returns it */
  222. Vector3 *V3Normalize(v) 
  223. Vector3 *v;
  224. {
  225. double len = V3Length(v);
  226.     if (len != 0.0) { v->x /= len;  v->y /= len; v->z /= len; }
  227.     return(v);
  228.     }
  229.  
  230. /* scales the input vector to the new length and returns it */
  231. Vector3 *V3Scale(v, newlen) 
  232. Vector3 *v;
  233. double newlen;
  234. {
  235. double len = V3Length(v);
  236.     if (len != 0.0) {
  237.     v->x *= newlen/len;   v->y *= newlen/len;  v->z *= newlen/len;
  238.     }
  239.     return(v);
  240.     }
  241.  
  242.  
  243. /* return vector sum c = a+b */
  244. Vector3 *V3Add(a, b, c)
  245. Vector3 *a, *b, *c;
  246. {
  247.     c->x = a->x+b->x;  c->y = a->y+b->y;  c->z = a->z+b->z;
  248.     return(c);
  249.     }
  250.     
  251. /* return vector difference c = a-b */
  252. Vector3 *V3Sub(a, b, c)
  253. Vector3 *a, *b, *c;
  254. {
  255.     c->x = a->x-b->x;  c->y = a->y-b->y;  c->z = a->z-b->z;
  256.     return(c);
  257.     }
  258.  
  259. /* return the dot product of vectors a and b */
  260. double V3Dot(a, b) 
  261. Vector3 *a, *b; 
  262. {
  263.     return((a->x*b->x)+(a->y*b->y)+(a->z*b->z));
  264.     }
  265.  
  266. /* linearly interpolate between vectors by an amount alpha */
  267. /* and return the resulting vector. */
  268. /* When alpha=0, result=lo.  When alpha=1, result=hi. */
  269. Vector3 *V3Lerp(lo, hi, alpha, result) 
  270. Vector3 *lo, *hi, *result; 
  271. double alpha;
  272. {
  273.     result->x = LERP(alpha, lo->x, hi->x);
  274.     result->y = LERP(alpha, lo->y, hi->y);
  275.     result->z = LERP(alpha, lo->z, hi->z);
  276.     return(result);
  277.     }
  278.  
  279. /* make a linear combination of two vectors and return the result. */
  280. /* result = (a * ascl) + (b * bscl) */
  281. Vector3 *V3Combine (a, b, result, ascl, bscl) 
  282. Vector3 *a, *b, *result;
  283. double ascl, bscl;
  284. {
  285.     result->x = (ascl * a->x) + (bscl * b->x);
  286.     result->y = (ascl * a->y) + (bscl * b->y);
  287.     result->z = (ascl * a->z) + (bscl * b->z);
  288.     return(result);
  289.     }
  290.  
  291.  
  292. /* multiply two vectors together component-wise and return the result */
  293. Vector3 *V3Mul (a, b, result) 
  294. Vector3 *a, *b, *result;
  295. {
  296.     result->x = a->x * b->x;
  297.     result->y = a->y * b->y;
  298.     result->z = a->z * b->z;
  299.     return(result);
  300.     }
  301.  
  302. /* return the distance between two points */
  303. double V3DistanceBetween2Points(a, b)
  304. Point3 *a, *b;
  305. {
  306. double dx = a->x - b->x;
  307. double dy = a->y - b->y;
  308. double dz = a->z - b->z;
  309.     return(sqrt((dx*dx)+(dy*dy)+(dz*dz)));
  310.     }
  311.  
  312. /* return the cross product c = a cross b */
  313. Vector3 *V3Cross(a, b, c)
  314. Vector3 *a, *b, *c;
  315. {
  316.     c->x = (a->y*b->z) - (a->z*b->y);
  317.     c->y = (a->z*b->x) - (a->x*b->z);
  318.     c->z = (a->x*b->y) - (a->y*b->x);
  319.     return(c);
  320.     }
  321.  
  322. /* create, initialize, and return a new vector */
  323. Vector3 *V3New(x, y, z)
  324. double x, y, z;
  325. {
  326. Vector3 *v = NEWTYPE(Vector3);
  327.     v->x = x;  v->y = y;  v->z = z;
  328.     return(v);
  329.     }
  330.  
  331. /* create, initialize, and return a duplicate vector */
  332. Vector3 *V3Duplicate(a)
  333. Vector3 *a;
  334. {
  335. Vector3 *v = NEWTYPE(Vector3);
  336.     v->x = a->x;  v->y = a->y;  v->z = a->z;
  337.     return(v);
  338.     }
  339.  
  340.     
  341. /* multiply a point by a matrix and return the transformed point */
  342. Point3 *V3MulPointByMatrix(pin, m, pout)
  343. Point3 *pin, *pout;
  344. Matrix3 *m;
  345. {
  346.     pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) + 
  347.          (pin->z * m->element[2][0]);
  348.     pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) + 
  349.          (pin->z * m->element[2][1]);
  350.     pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) + 
  351.          (pin->z * m->element[2][2]);
  352.     return(pout);
  353.     }
  354.  
  355. /* multiply a point by a projective matrix and return the transformed point */
  356. Point3 *V3MulPointByProjMatrix(pin, m, pout)
  357. Point3 *pin, *pout;
  358. Matrix4 *m;
  359. {
  360. double w;
  361.     pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) + 
  362.          (pin->z * m->element[2][0]) + m->element[3][0];
  363.     pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) + 
  364.          (pin->z * m->element[2][1]) + m->element[3][1];
  365.     pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) + 
  366.          (pin->z * m->element[2][2]) + m->element[3][2];
  367.     w =    (pin->x * m->element[0][3]) + (pin->y * m->element[1][3]) + 
  368.          (pin->z * m->element[2][3]) + m->element[3][3];
  369.     if (w != 0.0) { pout->x /= w;  pout->y /= w;  pout->z /= w; }
  370.     return(pout);
  371.     }
  372.  
  373. /* multiply together matrices c = ab */
  374. /* note that c must not point to either of the input matrices */
  375. Matrix4 *V3MatMul(a, b, c)
  376. Matrix4 *a, *b, *c;
  377. {
  378. int i, j, k;
  379.     for (i=0; i<4; i++) {
  380.         for (j=0; j<4; j++) {
  381.             c->element[i][j] = 0;
  382.             for (k=0; k<4; k++) c->element[i][j] += 
  383.                 a->element[i][k] * b->element[k][j];
  384.             }
  385.         }
  386.     return(c);
  387.     }
  388.  
  389. /* binary greatest common divisor by Silver and Terzian.  See Knuth */
  390. /* both inputs must be >= 0 */
  391. gcd(u, v)
  392. int u, v;
  393. {
  394. int t, f;
  395.     if ((u<0) || (v<0)) return(1); /* error if u<0 or v<0 */
  396.     f = 1;
  397.     while ((0 == (u%2)) && (0 == (v%2))) {
  398.         u>>=1;  v>>=1,  f*=2;
  399.         }
  400.     if (u&01) { t = -v;  goto B4; } else { t = u; }
  401.     B3: if (t > 0) { t >>= 1; } else { t = -((-t) >> 1); }
  402.     B4: if (0 == (t%2)) goto B3;
  403.  
  404.     if (t > 0) u = t; else v = -t;
  405.     if (0 != (t = u - v)) goto B3;
  406.     return(u*f);
  407.     }    
  408.  
  409. /***********************/
  410. /*   Useful Routines   */
  411. /***********************/
  412.  
  413. /* return roots of ax^2+bx+c */
  414. /* stable algebra derived from Numerical Recipes by Press et al.*/
  415. int quadraticRoots(a, b, c, roots)
  416. double a, b, c, *roots;
  417. {
  418. double d, q;
  419. int count = 0;
  420.     d = (b*b)-(4*a*c);
  421.     if (d < 0.0) { *roots = *(roots+1) = 0.0;  return(0); }
  422.     q =  -0.5 * (b + (SGN(b)*sqrt(d)));
  423.     if (a != 0.0)  { *roots++ = q/a; count++; }
  424.     if (q != 0.0) { *roots++ = c/q; count++; }
  425.     return(count);
  426.     }
  427.  
  428.  
  429. /* generic 1d regula-falsi step.  f is function to evaluate */
  430. /* interval known to contain root is given in left, right */
  431. /* returns new estimate */
  432. double RegulaFalsi(f, left, right)
  433. double (*f)(), left, right;
  434. {
  435. double d = (*f)(right) - (*f)(left);
  436.     if (d != 0.0) return (right - (*f)(right)*(right-left)/d);
  437.     return((left+right)/2.0);
  438.     }
  439.  
  440. /* generic 1d Newton-Raphson step. f is function, df is derivative */
  441. /* x is current best guess for root location. Returns new estimate */
  442. double NewtonRaphson(f, df, x)
  443. double (*f)(), (*df)(), x;
  444. {
  445. double d = (*df)(x);
  446.     if (d != 0.0) return (x-((*f)(x)/d));
  447.     return(x-1.0);
  448.     }
  449.  
  450.  
  451. /* hybrid 1d Newton-Raphson/Regula Falsi root finder. */
  452. /* input function f and its derivative df, an interval */
  453. /* left, right known to contain the root, and an error tolerance */
  454. /* Based on Blinn */
  455. double findroot(left, right, tolerance, f, df)
  456. double left, right, tolerance;
  457. double (*f)(), (*df)();
  458. {
  459. double newx = left;
  460.     while (ABS((*f)(newx)) > tolerance) {
  461.         newx = NewtonRaphson(f, df, newx);
  462.         if (newx < left || newx > right) 
  463.             newx = RegulaFalsi(f, left, right);
  464.         if ((*f)(newx) * (*f)(left) <= 0.0) right = newx;  
  465.             else left = newx;
  466.         }
  467.     return(newx);
  468.     } 
  469.  
  470.