home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / general / modelers / geomview / source.lha / Geomview / src / lib / geometry / hpoint3 / hpoint3a.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-03-25  |  4.9 KB  |  265 lines
  1. /*
  2.  * hpoint3a.c: linear algebra routines for hpoints
  3.  */
  4.  
  5. #include <math.h>
  6. #include <stdio.h>
  7. #include "hg4.h"
  8. #include "point3.h"
  9. #include "hpoint3.h"
  10. #include "hplane3.h"
  11. #include "hline3.h"
  12. #include "transform3.h"
  13. #include "tolerance.h"
  14.  
  15. /* inner product of R4 */
  16. HPt3Coord
  17. HPt3R40Dot(HPoint3 *a, HPoint3 *b)
  18. {
  19.   return a->x*b->x + a->y*b->y + a->z*b->z + a->w*b->w;
  20. }
  21.  
  22. /* inner product of R(3,1) */
  23. HPt3Coord
  24. HPt3R31Dot(HPoint3 *a, HPoint3 *b)
  25. {
  26.   return a->x*b->x + a->y*b->y + a->z*b->z - a->w*b->w;
  27. }
  28.  
  29. /* inner product of R(3,0) */
  30. HPt3Coord
  31. HPt3R30Dot(HPoint3 *a, HPoint3 *b)
  32. {
  33.   double w2 = a->w*b->w;
  34.   if (w2 == 1.0 || w2 == 0.0)
  35.     return a->x*b->x + a->y*b->y + a->z*b->z;
  36.   else
  37.     return (a->x*b->x + a->y*b->y + a->z*b->z) / w2;
  38. }
  39.  
  40. HPt3Coord
  41. HPt3SpaceDot(HPoint3 *a, HPoint3 *b, int space)
  42. {
  43.   switch (space) {
  44.   case TM_EUCLIDEAN:
  45.   default:
  46.     return HPt3R30Dot(a,b);
  47.     break;
  48.   case TM_HYPERBOLIC:
  49.     return HPt3R31Dot(a,b);
  50.     break;
  51.   case TM_SPHERICAL:
  52.     return HPt3R40Dot(a,b);
  53.     break;
  54.   }
  55. }
  56.  
  57. /* normalize a point a in R4 so that <a,a> = 1 */
  58. void
  59. HPt3R40Normalize(HPoint3 *a)
  60. {
  61.   float len = sqrt( (double)(HPt3R40Dot(a,a)) );
  62.   if (len > 0) {
  63.     len = 1 / len;
  64.     a->x *= len;
  65.     a->y *= len;
  66.     a->z *= len;
  67.     a->w *= len;
  68.   }
  69. }
  70.  
  71. /* normalize a point a in R(3,1) so that <a,a> = sign(<a,a>) */
  72. void
  73. HPt3R31Normalize(HPoint3 *a)
  74. {
  75.   float len = sqrt( fabs( (double)(HPt3R31Dot(a,a)) ) );
  76.   if (len > 0) {
  77.     len = 1 / len;
  78.     a->x *= len;
  79.     a->y *= len;
  80.     a->z *= len;
  81.     a->w *= len;
  82.   }
  83. }
  84.  
  85. /* normalize a point a in R(3,0) so that <a,a> = 1 */
  86. void
  87. HPt3R30Normalize(HPoint3 *a)
  88. {
  89.   float len = sqrt( (double)(HPt3R30Dot(a,a)) );
  90.   if (len > 0) {
  91.     len = 1 / len;
  92.     a->x *= len;
  93.     a->y *= len;
  94.     a->z *= len;
  95.   }
  96. }
  97.  
  98. void
  99. HPt3SpaceNormalize(HPoint3 *a, int space)
  100. {
  101.   switch (space) {
  102.   case TM_EUCLIDEAN:
  103.   default:
  104.     HPt3R30Normalize(a);
  105.     break;
  106.   case TM_HYPERBOLIC:
  107.     HPt3R31Normalize(a);
  108.     break;
  109.   case TM_SPHERICAL:
  110.     HPt3R40Normalize(a);
  111.     break;
  112.   }
  113. }
  114.  
  115. HPt3Coord
  116. HPt3HypDistance(HPoint3 *a, HPoint3 *b)
  117. {
  118.   float aa, bb, ab;
  119.   aa = HPt3R31Dot(a,a);
  120.   bb = HPt3R31Dot(b,b);
  121.   ab = HPt3R31Dot(a,b);
  122.   return acosh( fabs(ab / sqrt( aa * bb ) ));
  123. }
  124.  
  125. HPt3Coord
  126. HPt3Distance( a, b )
  127.     HPoint3 *a, *b;
  128. {
  129.     float dx, dy, dz;
  130.     float w1w2;
  131.  
  132.     w1w2 = a->w * b->w;
  133.     if( w1w2 == 0. )
  134.     return 0.;
  135.  
  136.     dx = b->w * a->x - b->x * a->w;
  137.     dy = b->w * a->y - b->y * a->w;
  138.     dz = b->w * a->z - b->z * a->w;
  139.  
  140.     return (sqrt( dx*dx + dy*dy + dz*dz )) / w1w2;
  141. }
  142.  
  143. HPt3Coord
  144. HPt3SphDistance(HPoint3 *a, HPoint3 *b)
  145. {
  146.   return acos( HPt3R40Dot(a,b) / sqrt( HPt3R40Dot(a,a) * HPt3R40Dot(b,b) ) );
  147. }
  148.  
  149. HPt3Coord
  150. HPt3SpaceDistance(HPoint3 *a, HPoint3 *b, int space)
  151. {
  152.   switch (space) {
  153.   case TM_EUCLIDEAN:
  154.   default:
  155.     return HPt3Distance(a,b);
  156.     break;
  157.   case TM_HYPERBOLIC:
  158.     return HPt3HypDistance(a,b);
  159.     break;
  160.   case TM_SPHERICAL:
  161.     return HPt3SphDistance(a,b);
  162.     break;
  163.   }
  164. }
  165.  
  166. void
  167. HPt3Sub(HPoint3 *a, HPoint3 *b, HPoint3 *aminusb)
  168. {
  169.   aminusb->x = a->x - b->x;
  170.   aminusb->y = a->y - b->y;
  171.   aminusb->z = a->z - b->z;
  172.   aminusb->w = a->w - b->w;
  173. }
  174.  
  175. void
  176. HPt3Scale(HPt3Coord s, HPoint3 *a, HPoint3 *sa)
  177. {
  178.   sa->x = s * a->x;
  179.   sa->y = s * a->y;
  180.   sa->z = s * a->z;
  181.   sa->w = s * a->w;
  182. }
  183.  
  184. void
  185. HPt3GramSchmidt(HPoint3 *base, HPoint3 *v)
  186. {
  187.   HPt3SpaceGramSchmidt(base, v, TM_EUCLIDEAN);
  188. }
  189.  
  190. void
  191. HPt3HypGramSchmidt(HPoint3 *base, HPoint3 *v)
  192. {
  193.   HPt3SpaceGramSchmidt(base, v, TM_HYPERBOLIC);
  194. }
  195.  
  196. void
  197. HPt3SphGramSchmidt(HPoint3 *base, HPoint3 *v)
  198. {
  199.   HPt3SpaceGramSchmidt(base, v, TM_SPHERICAL);
  200. }
  201.  
  202. /* Modifies v to arrange that <base,v> = 0,
  203.    i.e. v is a tangent vector based at base */
  204. void
  205. HPt3SpaceGramSchmidt(HPoint3 *base, HPoint3 *v, int space)
  206. {
  207.   HPt3Coord d,e;
  208.   HPoint3 tmp;
  209.  
  210.   d = HPt3SpaceDot(base,v,space);
  211.   e = HPt3SpaceDot(base,base,space);
  212.   if (e == 0.0) {
  213.     fprintf(stderr, "GramSchmidt: invalid base point.\n");
  214.     e = 1.0;
  215.   }
  216.   HPt3Scale((HPt3Coord)(d / e), base, &tmp);
  217.   HPt3Sub(v, &tmp, v);
  218. }
  219.  
  220.  
  221. #if 0
  222.  
  223. /* This stuff doesn't compile yet; in progress: */
  224.  
  225.  
  226. HPt3Coord
  227. HPt3Angle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2)
  228. {
  229.   /*
  230.    * I'm not sure that this will work in the euclidean case!!! mbp
  231.    */
  232.   return HPt3SpaceAngle(base, v1, v2, TM_EUCLIDEAN);
  233. }
  234.  
  235. HPt3Coord
  236. HPt3HypAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2)
  237. {
  238.   return HPt3SpaceAngle(base, v1, v2, TM_HYPERBOLIC);
  239. }
  240.  
  241. HPt3Coord
  242. HPt3SphAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2)
  243. {
  244.   return HPt3SpaceAngle(base, v1, v2, TM_SPHERICAL);
  245. }
  246.  
  247. HPt3Coord
  248. HPt3SpaceAngle(HPoint3 *base, HPoint3 *v1, HPoint3 *v2, int space)
  249. {
  250.   double d, n;
  251.   HPoint3 v1n = *v1;
  252.   HPoint3 v2n = *v2;
  253.   HPt3SpaceGramSchmidt(base, &v1n, space);
  254.   HPt3SpaceGramSchmidt(base, &v2n, space);
  255.   d = HPt3SpaceDot(&v1n,&v1n,space) * HPt3SpaceDot(&v2n,&v2n,space);
  256.   if (d <= 0.0) {
  257.     fprintf(stderr,"HPt3SpaceAngle: invalid denominator\n");
  258.     return 0.0;
  259.   }
  260.   n = HPt3SpaceDot(&v1n, &v2n, space);
  261.   return acos( n / sqrt(d) );
  262. }
  263.  
  264. #endif
  265.