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