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