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