开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
Pnt3.h
资源名称：scanalyze-1.0.3_source_code.rar　[点击查看]
上传用户：kellyonhid
上传日期：2013-10-12
资源大小：932k
文件大小：28k
源码类别：

3D图形编程

开发平台：

Visual C++

Pnt3.h：源码内容
  1. //###############################################################
  2. // Pnt3.h
  3. // Kari Pulli
  4. // 11/08/1996
  5. //###############################################################
  7. #ifndef _pnt3_h
  8. #define _pnt3_h
  10. #include<math.h>
  11. #include<assert.h>
  12. #include<iostream.h>
  13. #ifdef WIN32
  14. #include<float.h>
  15. #endif
  16. #ifdef sgi
  17. #include<ieeefp.h>
  18. #endif
  20. #ifdef sun
  21. #define sqrtf(x) sqrt(x)
  22. #endif
  24. #define SHOW(X) cout << #X " = " << X << endl
  26. class Pnt3 {
  27. protected:
  28.   float v[3];
  29. public: 
  30.   Pnt3(float a=0.0, float b=0.0, float c=0.0) 
  31.     { v[0] = a, v[1] = b, v[2] = c;}
  32.   Pnt3(const float *a)    { v[0] = a[0], v[1] = a[1], v[2] = a[2];}
  33.   Pnt3(const double *a)   { v[0] = a[0], v[1] = a[1], v[2] = a[2];}
  35.   Pnt3 &set(float a=0.0, float b=0.0, float c=0.0) 
  36.     { v[0] = a, v[1] = b, v[2] = c; return *this; }
  37.   Pnt3 &set(const float *a) 
  38.     { v[0] = a[0], v[1] = a[1], v[2] = a[2]; return *this; }
  39.   Pnt3 &set(const double *a)
  40.     { v[0] = a[0], v[1] = a[1], v[2] = a[2]; return *this; }
  41.   
  42.   // linear interpolation
  43.   Pnt3 &lerp(float t, const Pnt3 &a, const Pnt3 &b)
  44.     {
  45.       float u = 1.0 - t;
  46.       v[0]=u*a[0]+t*b[0]; 
  47.       v[1]=u*a[1]+t*b[1]; 
  48.       v[2]=u*a[2]+t*b[2];
  49.       return *this;
  50.     }
  52.   Pnt3 &set_max(const Pnt3 &p)
  53.     { 
  54.       if (p.v[0] > v[0]) v[0] = p.v[0];
  55.       if (p.v[1] > v[1]) v[1] = p.v[1];
  56.       if (p.v[2] > v[2]) v[2] = p.v[2];
  57.       return *this;
  58.     }
  59.   Pnt3 &set_min(const Pnt3 &p)
  60.     { 
  61.       if (p.v[0] < v[0]) v[0] = p.v[0];
  62.       if (p.v[1] < v[1]) v[1] = p.v[1];
  63.       if (p.v[2] < v[2]) v[2] = p.v[2];
  64.       return *this;
  65.     }
  67. #ifdef WIN32
  68.   bool  isfinite(void) { return _finite(v[0]) && _finite(v[1]) && _finite(v[2]); }
  69. #else
  70.   bool  isfinite(void) { return finite(v[0]) && finite(v[1]) && finite(v[2]); }
  71. #endif
  73.   Pnt3  operator-() const       { return Pnt3(-v[0],-v[1],-v[2]); }
  74.   Pnt3& operator+=(const Pnt3 &);
  75.   Pnt3& operator-=(const Pnt3 &);
  76.   Pnt3& operator*=(const float &);
  77.   Pnt3& operator/=(const float &);
  78.   int   operator==(const Pnt3 &);
  79.   int   operator!=(const Pnt3 &);
  80.   operator const float *(void) const   { return v; }
  81.   operator       float *(void)         { return v; }
  82.   operator const char  *(void) const   { return (char *)v; }
  83.   operator       char  *(void)         { return (char *)v; }
  84.   float& operator[](int i)             { return v[i]; }
  85.   const float& operator[](int i) const { return v[i]; }
  86.   friend ostream& operator<<(ostream &out, const Pnt3 &a);
  87.   friend istream& operator>>(istream &in, Pnt3 &a);
  88.   // operators that are not part of class: +,-,*,/
  90.   float         norm(void) const;
  91.   float         norm2(void) const;
  92.   Pnt3 &        normalize(void);
  93.   Pnt3 &        set_norm(float len);
  94.   friend float  dist(const Pnt3 &, const Pnt3 &);
  95.   friend float  dist2(const Pnt3 &, const Pnt3 &);
  96.   friend float  dist_2d(const Pnt3 &, const Pnt3 &);
  97.   friend float  dist2_2d(const Pnt3 &, const Pnt3 &);
  98.   friend float  dist_manhattan(const Pnt3 &, const Pnt3 &);
  99.   friend float  dist2_lineseg(const Pnt3 &, const Pnt3 &, const Pnt3 &);
  100.   friend float  dist2_tri(const Pnt3 &, 
  101.   const Pnt3 &, const Pnt3 &, const Pnt3 &);
  103.   friend bool   closer_on_lineseg(const Pnt3 &, Pnt3 &, 
  104.   const Pnt3 &, 
  105.   const Pnt3 &, float &);
  106.   friend bool   closer_on_tri(const Pnt3 &, Pnt3 &, const Pnt3 &, 
  107.       const Pnt3 &, const Pnt3 &, float &);
  109.   friend float  dot(const Pnt3 &a, const Pnt3 &b);
  110.   friend Pnt3   cross(const Pnt3 &a, const Pnt3 &b);
  111.   friend Pnt3   cross(const Pnt3 &a, const Pnt3 &b,
  112.       const Pnt3 &c);
  113.   friend Pnt3   normal(const Pnt3 &, const Pnt3 &, const Pnt3 &);
  114.   friend float  det(const Pnt3 &, const Pnt3 &, const Pnt3 &);
  115.   friend void   line_plane_X(const Pnt3& p, const Pnt3& dir,
  116.      const Pnt3& t1, const Pnt3& t2,
  117.      const Pnt3& t3,
  118.      Pnt3 &x, float &dist);
  119.   friend void   line_plane_X(const Pnt3& p, const Pnt3& dir,
  120.      const Pnt3& nrm, float d,
  121.      Pnt3 &x, float &dist);
  123.   friend void   bary(const Pnt3& p,  const Pnt3& t1,
  124.      const Pnt3& t2, const Pnt3& t3,
  125.      float &b1, float &b2, float &b3);
  126.   friend void   bary_fast(const Pnt3& p, const Pnt3& n, 
  127.   const Pnt3 &t0, const Pnt3& v1, const Pnt3& v2,
  128.   float &b1, float &b2, float &b3);
  129.   friend void   bary(const Pnt3& p,  const Pnt3& dir,
  130.      const Pnt3& t1, const Pnt3& t2, 
  131.      const Pnt3& t3,
  132.      float &b1, float &b2, float &b3);
  133.   friend int    above_plane(const Pnt3& p, const Pnt3& a, 
  134.     const Pnt3& b, const Pnt3& c);
  136.   float         smallest_circle(const Pnt3 &, const Pnt3 &, const Pnt3 &);
  138.   // functions used for closest point searching in KDtrees and octrees
  139.   friend bool   ball_within_bounds(const Pnt3 &, float,
  140.    const Pnt3 &, float);
  141.   friend bool   ball_within_bounds(const Pnt3 &, float,
  142.    const Pnt3 &, const Pnt3 &);
  143.   friend bool   bounds_overlap_ball(const Pnt3 &, float,
  144.     const Pnt3 &, float);
  145.   friend bool   bounds_overlap_ball(const Pnt3 &, float,
  146.     const Pnt3 &, const Pnt3 &);
  147.   friend bool   spheres_intersect(const Pnt3 &, const Pnt3 &,
  148.   float, float);
  149.   friend bool   lines_intersect(const Pnt3 &p1,
  150. const Pnt3 &p2,
  151. const Pnt3 &p3,
  152. const Pnt3 &p4,
  153. Pnt3 &isect);
  155.   // rigid transformations only (rot + trans)
  156.   Pnt3 &        xform(float m[16]);// OpenGL matrix: p . M = p'
  157.   Pnt3 &        xform(double m[16]);// OpenGL matrix: p . M = p'
  158.   Pnt3 &        xform(float r[3][3], float  t[3]);
  159.   Pnt3 &        xform(float r[3][3], double t[3]);
  160.   Pnt3 &        invxform(float m[16]);// OpenGL matrix: p . M = p'
  161.   Pnt3 &        invxform(double m[16]);// OpenGL matrix: p . M = p'
  162.   Pnt3 &        invxform(float r[3][3], float  t[3]);
  163.   Pnt3 &        invxform(float r[3][3], double t[3]);
  165.   Pnt3 & setXformed(const Pnt3 &p, float r[3][3], float t[3]);
  166.   Pnt3 & setRotated(const Pnt3 &p, float r[3][3]);
  167. };
  170. inline Pnt3& 
  171. Pnt3::operator+=(const Pnt3 &a)
  172. {
  173.   v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2];
  174.   return *this;
  175. }
  177. inline Pnt3& 
  178. Pnt3::operator-=(const Pnt3 &a)
  179. {
  180.   v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2];
  181.   return *this;
  182. }
  184. inline Pnt3& 
  185. Pnt3::operator*=(const float &a)
  186. {
  187.   v[0] *= a; v[1] *= a; v[2] *= a;
  188.   return *this;
  189. }
  191. inline Pnt3& 
  192. Pnt3::operator/=(const float &a)
  193. {
  194.   float tmp = 1.0f / a;
  195.   v[0] *= tmp; v[1] *= tmp; v[2] *= tmp;
  196.   return *this;
  197. }
  199. inline int
  200. Pnt3::operator==(const Pnt3 &a)
  201. {
  202.   return (a.v[0]==v[0] && a.v[1]==v[1] && a.v[2]==v[2]);
  203. }
  205. inline int
  206. Pnt3::operator!=(const Pnt3 &a)
  207. {
  208.   return (a.v[0]!=v[0] || a.v[1]!=v[1] || a.v[2]!=v[2]);
  209. }
  211. inline Pnt3
  212. operator+(const Pnt3 &a, const Pnt3 &b)
  213. {
  214.   Pnt3 tmp = a; 
  215.   return (tmp += b);
  216. }
  218. inline Pnt3
  219. operator-(const Pnt3 &a, const Pnt3 &b)
  220. {
  221.   Pnt3 tmp = a; 
  222.   return (tmp -= b);
  223. }
  225. inline Pnt3
  226. operator*(const Pnt3 &a, const float &b)
  227. {
  228.   Pnt3 tmp = a; 
  229.   return (tmp *= b);
  230. }
  232. inline Pnt3
  233. operator*(const float &a, const Pnt3 &b)
  234. {
  235.   Pnt3 tmp = b; 
  236.   return (tmp *= a);
  237. }
  239. inline Pnt3
  240. operator/(const Pnt3 &a, const float &b)
  241. {
  242.   Pnt3 tmp = a; 
  243.   return (tmp /= b);
  244. }
  246. inline ostream& 
  247. operator<<(ostream &out, const Pnt3 &a)
  248. { 
  249.   return out << "["<< a.v[0] <<" "<< a.v[1] <<" "<< a.v[2] << "]";
  250. }
  252. inline istream& 
  253. operator>>(istream &in, Pnt3 &a)
  254. {
  255.   char c = 0;
  256.   in >> ws >> c;
  257.   if (c == '[') {
  258.     in >> a.v[0] >> a.v[1] >> a.v[2];
  259.     in >> c;
  260.     if (c != ']') in.clear(ios::badbit);
  261.   } else {
  262.     in.putback(c);
  263.     in >> a.v[0] >> a.v[1] >> a.v[2];
  264.   }
  265.   return in;
  266. }
  268. inline float 
  269. Pnt3::norm() const
  270. { 
  271.   return sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
  272. }
  274. inline float 
  275. Pnt3::norm2() const
  276. { 
  277.   return v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
  278. }
  280. inline Pnt3 &
  281. Pnt3::normalize(void)
  282. {
  283.   float a = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
  284.   if (a != 0) { 
  285.     a = 1.0/sqrtf(a); 
  286.     v[0] *= a; v[1] *= a; v[2] *= a; 
  287.   }
  288.   return *this;
  289. }
  291. inline Pnt3 &
  292. Pnt3::set_norm(float len)
  293. {
  294.   float a = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
  295.   if (a != 0) { 
  296.     a = len/sqrtf(a); 
  297.     v[0] *= a; v[1] *= a; v[2] *= a; 
  298.   }
  299.   return *this;
  300. }
  302. inline float
  303. dist2(const Pnt3 &a, const Pnt3 &b)
  304. {
  305.   float x = a.v[0]-b.v[0];
  306.   float y = a.v[1]-b.v[1];
  307.   float z = a.v[2]-b.v[2];
  308.   return x*x + y*y + z*z;
  309. }
  311. inline float
  312. dist(const Pnt3 &a, const Pnt3 &b)
  313. {
  314.   return sqrtf(dist2(a,b));
  315. }
  317. inline float
  318. dist2_2d(const Pnt3 &a, const Pnt3 &b)
  319. {
  320.   float x = a.v[0]-b.v[0];
  321.   float y = a.v[1]-b.v[1];
  322.   return x*x + y*y;
  323. }
  325. inline float
  326. dist_2d(const Pnt3 &a, const Pnt3 &b)
  327. {
  328.   return sqrtf(dist2_2d(a,b));
  329. }
  331. inline float
  332. dist_manhattan(const Pnt3 &a, const Pnt3 &b)
  333. {
  334.   float d = fabs(a[0]-b[0]);
  335.   float t = fabs(a[1]-b[1]);
  336.   if (t > d) d = t;
  337.   t = fabs(a[2]-b[2]);
  338.   if (t > d) return t;
  339.   return d;
  340. }
  342. // distance from x to linesegment from a to b
  343. inline float  
  344. dist2_lineseg(const Pnt3 &x, const Pnt3 &a, const Pnt3 &b)
  345. {
  346.   Pnt3 ba = b; ba -= a;
  347.   Pnt3 xa = x; xa -= a;
  349.   float xa_ba = dot(xa,ba);
  350.   // if the dot product is negative, the point is closest to a
  351.   if (xa_ba < 0.0)   return dist2(x,a);
  353.   // if the dot product is greater than squared segment length,
  354.   // the point is closest to b
  355.   float nba2 = ba.norm2();
  356.   if (xa_ba >= nba2) return dist2(x,b);
  358.   // take the squared dist x-a, squared dot of x-a to unit b-a,
  359.   // use Pythagoras' rule
  360.   return xa.norm2() - xa_ba*xa_ba/nba2;
  361. }
  364. inline float
  365. dist2_tri(const Pnt3 &x, 
  366.   const Pnt3 &t1, const Pnt3 &t2, const Pnt3 &t3)
  367. {
  368.   // calculate the normal and distance from the plane
  369.   Pnt3 v1(t2.v[0]-t1.v[0], t2.v[1]-t1.v[1], t2.v[2]-t1.v[2]);
  370.   Pnt3 v2(t3.v[0]-t1.v[0], t3.v[1]-t1.v[1], t3.v[2]-t1.v[2]);
  371.   Pnt3 n = cross(v1,v2);
  372.   float n_inv_mag2 = 1.0/n.norm2();
  373.   float tmp  = (x.v[0]-t1.v[0])*n.v[0] + 
  374.                (x.v[1]-t1.v[1])*n.v[1] + 
  375.                (x.v[2]-t1.v[2])*n.v[2];
  376.   float distp2 = tmp * tmp * n_inv_mag2;
  378.   // calculate the barycentric coordinates of the point 
  379.   // (projected onto tri plane) with respect to v123
  380.   float b1,b2,b3;
  381.   float f = tmp*n_inv_mag2;
  382.   Pnt3  pp(x[0]-f*n[0], x[1]-f*n[1], x[2]-f*n[2]);
  383.   bary_fast(pp, n, t1,v1,v2, b1,b2,b3);
  385.   // all non-negative, the point is within the triangle
  386.   if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
  387.     return distp2;
  388.   }
  390.   // look at the signs of the barycentric coordinates
  391.   // if there are two negative signs, the positive
  392.   // one tells the vertex that's closest
  393.   // if there's one negative sign, the opposite edge
  394.   // (with endpoints) is closest
  396.   if (b1 < 0.0) {
  397.     if (b2 < 0.0) { 
  398.       return dist2(x,t3);
  399.     } else if (b3 < 0.0) {
  400.       return dist2(x,t2);
  401.     } else return dist2_lineseg(x, t2,t3);
  402.   } else if (b2 < 0.0) {
  403.     if (b3 < 0.0) {
  404.       return dist2(x,t1);
  405.     } else return dist2_lineseg(x, t1,t3);
  406.   } else   return dist2_lineseg(x, t1,t2);
  407. }
  410. inline bool
  411. closer_on_lineseg(const Pnt3 &x, Pnt3 &cp, const Pnt3 &a, 
  412.   const Pnt3 &b, float &d2)
  413. {
  414.   Pnt3 ba(b.v[0]-a.v[0], b.v[1]-a.v[1], b.v[2]-a.v[2]);
  415.   Pnt3 xa(x.v[0]-a.v[0], x.v[1]-a.v[1], x.v[2]-a.v[2]);
  417.   float xa_ba = dot(xa,ba);
  418.   // if the dot product is negative, the point is closest to a
  419.   if (xa_ba < 0.0) {
  420.     float nd = dist2(x,a);
  421.     if (nd < d2) { cp = a; d2 = nd; return true; }
  422.     return false;
  423.   }
  425.   // if the dot product is greater than squared segment length,
  426.   // the point is closest to b
  427.   float fact = xa_ba/ba.norm2();
  428.   if (fact >= 1.0) {
  429.     float nd = dist2(x,b);
  430.     if (nd < d2) { cp = b; d2 = nd; return true; }
  431.     return false;
  432.   }
  434.   // take the squared dist x-a, squared dot of x-a to unit b-a,
  435.   // use Pythagoras' rule
  436.   float nd = xa.norm2() - xa_ba*fact;
  437.   if (nd < d2) { 
  438.     d2 = nd;
  439.     cp.v[0] = a.v[0] + fact * ba.v[0];
  440.     cp.v[1] = a.v[1] + fact * ba.v[1];
  441.     cp.v[2] = a.v[2] + fact * ba.v[2];
  442.     return true;
  443.   }
  444.   return false;
  445. }
  446.  
  448. inline bool
  449. closer_on_tri(const Pnt3 &x, Pnt3 &cp, const Pnt3 &t1, 
  450.       const Pnt3 &t2, const Pnt3 &t3, float &d2)
  451. {
  452.   // calculate the normal and distance from the plane
  453.   Pnt3 v1(t2.v[0]-t1.v[0], t2.v[1]-t1.v[1], t2.v[2]-t1.v[2]);
  454.   Pnt3 v2(t3.v[0]-t1.v[0], t3.v[1]-t1.v[1], t3.v[2]-t1.v[2]);
  455.   Pnt3 n = cross(v1,v2);
  456.   float n_inv_mag2 = 1.0/n.norm2();
  457.   float tmp  = (x.v[0]-t1.v[0])*n.v[0] + 
  458.                (x.v[1]-t1.v[1])*n.v[1] + 
  459.                (x.v[2]-t1.v[2])*n.v[2];
  460.   float distp2 = tmp * tmp * n_inv_mag2;
  461.   if (distp2 >= d2) return false;
  463.   // calculate the barycentric coordinates of the point 
  464.   // (projected onto tri plane) with respect to v123
  465.   float b1,b2,b3;
  466.   float f = tmp*n_inv_mag2;
  467.   Pnt3  pp(x[0]-f*n[0], x[1]-f*n[1], x[2]-f*n[2]);
  468.   bary_fast(pp, n, t1,v1,v2, b1,b2,b3);
  470.   // all non-negative, the point is within the triangle
  471.   if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
  472.     d2 = distp2;
  473.     cp = pp;
  474.     return true;
  475.   }
  477.   // look at the signs of the barycentric coordinates
  478.   // if there are two negative signs, the positive
  479.   // one tells the vertex that's closest
  480.   // if there's one negative sign, the opposite edge
  481.   // (with endpoints) is closest
  483.   if (b1 < 0.0) {
  484.     if (b2 < 0.0) { 
  485.       float nd = dist2(x,t3);
  486.       if (nd < d2) { d2 = nd; cp = t3; return true; }
  487.       else         { return false; }
  488.     } else if (b3 < 0.0) {
  489.       float nd = dist2(x,t2);
  490.       if (nd < d2) { d2 = nd; cp = t2; return true; }
  491.       else         { return false; }
  492.     } else return closer_on_lineseg(x, cp, t2,t3, d2);
  493.   } else if (b2 < 0.0) {
  494.     if (b3 < 0.0) {
  495.       float nd = dist2(x,t1);
  496.       if (nd < d2) { d2 = nd; cp = t1; return true; }
  497.       else         { return false; }
  498.     } else return closer_on_lineseg(x, cp, t1,t3, d2);
  499.   } else   return closer_on_lineseg(x, cp, t1,t2, d2);
  500. }
  503. inline float 
  504. dot(const Pnt3 &a, const Pnt3 &b)
  505. { 
  506.   return (a.v[0]*b.v[0] + a.v[1]*b.v[1] + a.v[2]*b.v[2]);
  507. }
  510. inline Pnt3 
  511. cross(const Pnt3 &a, const Pnt3 &b)
  512. { 
  513.   return Pnt3(a.v[1]*b.v[2] - a.v[2]*b.v[1],
  514.       a.v[2]*b.v[0] - a.v[0]*b.v[2],
  515.       a.v[0]*b.v[1] - a.v[1]*b.v[0]);
  516. }
  519. // two vectors, a and b, starting from c
  520. inline Pnt3
  521. cross(const Pnt3 &a, const Pnt3 &b, const Pnt3 &c)
  522. {
  523.   float a0 = a.v[0]-c.v[0];
  524.   float a1 = a.v[1]-c.v[1];
  525.   float a2 = a.v[2]-c.v[2];
  526.   float b0 = b.v[0]-c.v[0];
  527.   float b1 = b.v[1]-c.v[1];
  528.   float b2 = b.v[2]-c.v[2];
  529.   return Pnt3(a1*b2 - a2*b1, a2*b0 - a0*b2, a0*b1 - a1*b0);
  530. }
  532. // get a normal from triangle ABC, points given in ccw order
  533. inline Pnt3 
  534. normal(const Pnt3 &a, const Pnt3 &b, const Pnt3 &c)
  535. { 
  536.   float a0 = a.v[0]-c.v[0];
  537.   float a1 = a.v[1]-c.v[1];
  538.   float a2 = a.v[2]-c.v[2];
  539.   float b0 = b.v[0]-c.v[0];
  540.   float b1 = b.v[1]-c.v[1];
  541.   float b2 = b.v[2]-c.v[2];
  542.   float x = a1*b2 - a2*b1;
  543.   float y = a2*b0 - a0*b2;
  544.   float z = a0*b1 - a1*b0;
  545.   float n = x*x+y*y+z*z;
  546.   if (n == 0) {
  547.     return Pnt3();
  548.   } else {
  549.     n = 1.0/sqrtf(n);
  550.     return Pnt3(x*n, y*n, z*n);
  551.   }
  552. }
  554. // determinant of 3 points
  555. inline float  
  556. det(const Pnt3 &a, const Pnt3 &b, const Pnt3 &c)
  557. {
  558.   return a[0]*(b[1]*c[2]-b[2]*c[1]) 
  559.     -    a[1]*(b[0]*c[2]-b[2]*c[0])
  560.     +    a[2]*(b[0]*c[1]-b[1]*c[0]);
  561. }
  563. // calculate the intersection of a line going through p
  564. // to direction dir with a plane spanned by t1,t2,t3
  565. // (modified from Graphics Gems, p.299)
  566. inline void
  567. line_plane_X(const Pnt3& p, const Pnt3& dir,
  568.      const Pnt3& t1, const Pnt3& t2, const Pnt3& t3, 
  569.      Pnt3 &x, float &dist)
  570. {
  571.   // note: normal doesn't need to be unit vector
  572.   Pnt3  nrm = cross(t1,t2,t3);
  573.   float tmp = dot(nrm,dir);
  574.   if (tmp == 0.0) {
  575.     cerr << "Cannot intersect plane with a parallel line" << endl;
  576.     return;
  577.   }
  578.   // d  = -dot(nrm,t1)
  579.   // t  = - (d + dot(p,nrm))/dot(dir,nrm)
  580.   // is = p + dir * t
  581.   x = dir;
  582.   dist = (dot(nrm,t1)-dot(nrm,p))/tmp;
  583.   x *= dist;
  584.   x += p;
  585.   if (dist < 0.0) dist = -dist;
  586. }
  588. inline void   
  589. line_plane_X(const Pnt3& p, const Pnt3& dir,
  590.     const Pnt3& nrm, float d, Pnt3 &x, float &dist)
  591. {
  592.   float tmp = dot(nrm,dir);
  593.   if (tmp == 0.0) {
  594.     cerr << "Cannot intersect plane with a parallel line" << endl;
  595.     return;
  596.   }
  597.   x = dir;
  598.   dist = -(d+dot(nrm,p))/tmp;
  599.   x *= dist;
  600.   x += p;
  601.   if (dist < 0.0) dist = -dist;
  602. }
  604. // calculate barycentric coordinates of the point p
  605. // on triangle t1 t2 t3
  606. inline void 
  607. bary(const Pnt3& p, 
  608.      const Pnt3& t1, const Pnt3& t2, const Pnt3& t3,
  609.      float &b1, float &b2, float &b3)
  610. {
  611.   // figure out the plane onto which to project the vertices
  612.   // by calculating a cross product and finding its largest dimension
  613.   // then use Cramer's rule to calculate two of the
  614.   // barycentric coordinates
  615.   // e.g., if the z coordinate is ignored, and v1 = t1-t3, v2 = t2-t3
  616.   // b1 = det[x[0] v2[0]; x[1] v2[1]] / det[v1[0] v2[0]; v1[1] v2[1]]
  617.   // b2 = det[v1[0] x[0]; v1[1] x[1]] / det[v1[0] v2[0]; v1[1] v2[1]]
  618.   float v10 = t1.v[0]-t3.v[0];
  619.   float v11 = t1.v[1]-t3.v[1];
  620.   float v12 = t1.v[2]-t3.v[2];
  621.   float v20 = t2.v[0]-t3.v[0];
  622.   float v21 = t2.v[1]-t3.v[1];
  623.   float v22 = t2.v[2]-t3.v[2];
  624.   float c[2];
  625.   c[0] = fabs(v11*v22 - v12*v21);
  626.   c[1] = fabs(v12*v20 - v10*v22);
  627.   int i = 0;
  628.   if (c[1] > c[0]) i = 1;
  629.   if (fabs(v10*v21 - v11*v20) > c[i]) {
  630.     // ignore z
  631.     float d = 1.0 / (v10*v21-v11*v20);
  632.     float x0 = (p.v[0]-t3.v[0]);
  633.     float x1 = (p.v[1]-t3.v[1]);
  634.     b1 = (x0*v21 - x1*v20) * d;
  635.     b2 = (v10*x1 - v11*x0) * d;
  636.   } else if (i==0) {
  637.     // ignore x
  638.     float d = 1.0 / (v11*v22-v12*v21);
  639.     float x0 = (p.v[1]-t3.v[1]);
  640.     float x1 = (p.v[2]-t3.v[2]);
  641.     b1 = (x0*v22 - x1*v21) * d;
  642.     b2 = (v11*x1 - v12*x0) * d;
  643.   } else {
  644.     // ignore y
  645.     float d = 1.0 / (v12*v20-v10*v22);
  646.     float x0 = (p.v[2]-t3.v[2]);
  647.     float x1 = (p.v[0]-t3.v[0]);
  648.     b1 = (x0*v20 - x1*v22) * d;
  649.     b2 = (v12*x1 - v10*x0) * d;
  650.   }
  651.   b3 = 1.0 - b1 - b2;
  652. }
  655. // calculate barycentric coordinates of the point p
  656. // (already on the triangle plane) with normal vector n
  657. // and two edge vectors v1 and v2, 
  658. // starting from a common vertex t0
  659. inline void 
  660. bary_fast(const Pnt3& p, const Pnt3& n, 
  661.   const Pnt3 &t0, const Pnt3& v1, const Pnt3& v2,
  662.   float &b1, float &b2, float &b3)
  663. {
  664.   // see bary above
  665.   int i = 0;
  666.   if (n.v[1] > n.v[0]) i = 1;
  667.   if (n.v[2] > n.v[i]) {
  668.     // ignore z
  669.     float d = 1.0 / (v1.v[0]*v2.v[1]-v1.v[1]*v2.v[0]);
  670.     float x0 = (p.v[0]-t0.v[0]);
  671.     float x1 = (p.v[1]-t0.v[1]);
  672.     b1 = (x0*v2.v[1] - x1*v2.v[0]) * d;
  673.     b2 = (v1.v[0]*x1 - v1.v[1]*x0) * d;
  674.   } else if (i==0) {
  675.     // ignore x
  676.     float d = 1.0 / (v1.v[1]*v2.v[2]-v1.v[2]*v2.v[1]);
  677.     float x0 = (p.v[1]-t0.v[1]);
  678.     float x1 = (p.v[2]-t0.v[2]);
  679.     b1 = (x0*v2.v[2] - x1*v2.v[1]) * d;
  680.     b2 = (v1.v[1]*x1 - v1.v[2]*x0) * d;
  681.   } else {
  682.     // ignore y
  683.     float d = 1.0 / (v1.v[2]*v2.v[0]-v1.v[0]*v2.v[2]);
  684.     float x0 = (p.v[2]-t0.v[2]);
  685.     float x1 = (p.v[0]-t0.v[0]);
  686.     b1 = (x0*v2.v[0] - x1*v2.v[2]) * d;
  687.     b2 = (v1.v[2]*x1 - v1.v[0]*x0) * d;
  688.   }
  689.   b3 = 1.0 - b1 - b2;
  690. }
  693. // calculate barycentric coordinates for the intersection of
  694. // a line starting from p, going to direction dir, and the plane
  695. // of the triangle t1 t2 t3
  696. inline void 
  697. bary(const Pnt3& p, const Pnt3& dir,
  698.      const Pnt3& t1, const Pnt3& t2, const Pnt3& t3,
  699.      float &b1, float &b2, float &b3)
  700. {
  701.   Pnt3 x; float d;
  702.   line_plane_X(p, dir, t1, t2, t3, x, d);
  703.   bary(x, t1,t2,t3, b1,b2,b3);
  704. }
  706. // is p above the plane spanned by triangle abc (ccw order)?
  707. inline int    
  708. above_plane(const Pnt3& p, const Pnt3& a, 
  709.     const Pnt3& b, const Pnt3& c)
  710. {
  711.   Pnt3 nrm = cross(a,b,c);
  712.   return dot(p,nrm) > dot(a,nrm);
  713. }
  717. // find the smallest circle that covers a,b,c
  718. // set self to the center, return radius
  719. inline float
  720. Pnt3::smallest_circle(const Pnt3 &A, const Pnt3 &B, const Pnt3 &C)
  721. {
  722.   Pnt3 a(B); a-=A;
  723.   Pnt3 b(C); b-=B;
  724.   Pnt3 c(A); c-=C;
  725.   float da = a.norm2();
  726.   float db = b.norm2();
  727.   float dc = c.norm2();
  728.   //const float *x;
  729.   if (da > db && da > dc) {
  730.     // da longest
  731.     if (da >= db + dc) {
  732.       // over 90 deg angle, solution is the center of the 
  733.       // longest edge
  734.       v[0] = .5 * (A.v[0]+B.v[0]);
  735.       v[1] = .5 * (A.v[1]+B.v[1]);
  736.       v[2] = .5 * (A.v[2]+B.v[2]);
  737.       return .5*sqrtf(da);
  738.     }
  739.   } else if (dc > db) {
  740.     // dc longest
  741.     if (dc >= db + da) {
  742.       // over 90 deg angle, solution is the center of the 
  743.       // longest edge
  744.       v[0] = .5 * (A.v[0]+C.v[0]);
  745.       v[1] = .5 * (A.v[1]+C.v[1]);
  746.       v[2] = .5 * (A.v[2]+C.v[2]);
  747.       return .5*sqrtf(dc);
  748.     }
  749.   } else {
  750.     // db longest
  751.     if (db >= da + dc) {
  752.       // over 90 deg angle, solution is the center of the 
  753.       // longest edge
  754.       v[0] = .5 * (B.v[0]+C.v[0]);
  755.       v[1] = .5 * (B.v[1]+C.v[1]);
  756.       v[2] = .5 * (B.v[2]+C.v[2]);
  757.       return .5*sqrtf(db);
  758.     }
  759.   }
  760.     
  761.   // solution is the circumcircle (see GGems 4 p.144)
  762.   Pnt3 aperp = cross(a, cross(a,b));
  763.   float fact = dot(b,c) / dot(aperp, c);
  764.   v[0] = A.v[0] + .5 * (a.v[0] + fact * aperp.v[0]);
  765.   v[1] = A.v[1] + .5 * (a.v[1] + fact * aperp.v[1]);
  766.   v[2] = A.v[2] + .5 * (a.v[2] + fact * aperp.v[2]);
  767.   return .5 * sqrtf(da*fact*fact+da);
  768. }
  772. // is the ball centered at b with radius r
  773. // fully within the box centered at bc, with radius br?
  774. inline bool
  775. ball_within_bounds(const Pnt3 &b, float r,
  776.    const Pnt3 &bc, float br)
  777. {
  778.   r -= br;
  779.   if ((b.v[0] - bc.v[0] <= r) ||
  780.       (bc.v[0] - b.v[0] <= r) ||
  781.       (b.v[1] - bc.v[1] <= r) ||
  782.       (bc.v[1] - b.v[1] <= r) ||
  783.       (b.v[2] - bc.v[2] <= r) ||
  784.       (bc.v[2] - b.v[2] <= r)) return false;
  785.   return true;
  786. }
  789. // is the ball centered at b with radius r
  790. // fully within the box centered from min to max?
  791. inline bool
  792. ball_within_bounds(const Pnt3 &b,
  793.    float r,
  794.    const Pnt3 &min, 
  795.    const Pnt3 &max)
  796. {
  797.   if ((b.v[0] - min.v[0] <= r) ||
  798.       (max.v[0] - b.v[0] <= r) ||
  799.       (b.v[1] - min.v[1] <= r) ||
  800.       (max.v[1] - b.v[1] <= r) ||
  801.       (b.v[2] - min.v[2] <= r) ||
  802.       (max.v[2] - b.v[2] <= r)) return false;
  803.   return true;
  804. }
  807. // does the ball centered at b, with radius r,
  808. // intersect the box centered at bc, with radius br?
  809. inline bool
  810. bounds_overlap_ball(const Pnt3 &b, float r,
  811.     const Pnt3 &bc, float br)
  812. {
  813.   float sum = 0.0, tmp;
  814.   if        ((tmp = bc.v[0]-br - b.v[0]) > 0.0) {
  815.     if (tmp>r) return false; sum += tmp*tmp; 
  816.   } else if ((tmp = b.v[0] - (bc.v[0]+br)) > 0.0) { 
  817.     if (tmp>r) return false; sum += tmp*tmp; 
  818.   }
  819.   if        ((tmp = bc.v[1]-br - b.v[1]) > 0.0) {
  820.     if (tmp>r) return false; sum += tmp*tmp; 
  821.   } else if ((tmp = b.v[1] - (bc.v[1]+br)) > 0.0) { 
  822.     if (tmp>r) return false; sum += tmp*tmp; 
  823.   }
  824.   if        ((tmp = bc.v[2]-br - b.v[2]) > 0.0) {
  825.     if (tmp>r) return false; sum += tmp*tmp; 
  826.   } else if ((tmp = b.v[2] - (bc.v[2]+br)) > 0.0) { 
  827.     if (tmp>r) return false; sum += tmp*tmp; 
  828.   }
  829.   return (sum < r*r);
  830. }
  833. inline bool 
  834. bounds_overlap_ball(const Pnt3 &b,
  835.     float r,
  836.     const Pnt3 &min, 
  837.     const Pnt3 &max)
  838. {
  839.   float sum = 0.0, tmp;
  840.   if        (b.v[0] < min.v[0]) { 
  841.     tmp = min.v[0] - b.v[0]; if (tmp>r) return false; sum+=tmp*tmp;
  842.   } else if (b.v[0] > max.v[0]) { 
  843.     tmp = b.v[0] - max.v[0]; if (tmp>r) return false; sum+=tmp*tmp;
  844.   }
  845.   if        (b.v[1] < min.v[1]) { 
  846.     tmp = min.v[1] - b.v[1]; sum+=tmp*tmp;
  847.   } else if (b.v[1] > max.v[1]) { 
  848.     tmp = b.v[1] - max.v[1]; sum+=tmp*tmp;
  849.   }
  850.   r *= r;
  851.   if (sum > r) return false;
  852.   if        (b.v[2] < min.v[2]) { 
  853.     tmp = min.v[2] - b.v[2]; sum+=tmp*tmp;
  854.   } else if (b.v[2] > max.v[2]) { 
  855.     tmp = b.v[2] - max.v[2]; sum+=tmp*tmp;
  856.   }
  857.   return (sum < r);
  858. }
  862. // this function tests for the intersection
  863. // of two spheres, for one of which we have the
  864. // squared radius
  865. inline bool
  866. spheres_intersect(const Pnt3 &c1, const Pnt3 &c2,
  867.   float r1sqr, float r2)
  868. {
  869.   float x = c1.v[0]-c2.v[0];
  870.   float y = c1.v[1]-c2.v[1];
  871.   float z = c1.v[2]-c2.v[2];
  873.   // try to avoid square root
  874.   //float r2sqr = r2*r2;
  875.   //float R2 = (x*x + y*y + z*z - r1sqr - r2sqr)*.5;
  876.   float R2 = (x*x + y*y + z*z - r1sqr - r2*r2);
  877.   // check against underestimate
  878.   if (R2 < 0.0) return true;
  880.   /*
  881.   // check against overestimate
  882.   //R2 *= .5;
  883.   if (r1sqr > r2sqr) {
  884.     if (R2 > r1sqr) return false;
  885.   } else {
  886.     if (R2 > r2sqr) return false;
  887.   }
  888.   */
  889.   // had to take square root...
  890.   return (R2 < 2.0*sqrtf(r1sqr)*r2);  
  891. }
  894. //GGems II, p. 142
  895. inline bool
  896. lines_intersect(const Pnt3 &p1,
  897. const Pnt3 &p2,
  898. const Pnt3 &p3,
  899. const Pnt3 &p4,
  900. Pnt3 &isect)
  901. {
  902.   Pnt3 a = p2 - p1;
  903.   Pnt3 b = p4 - p3;
  904.   Pnt3 axb = cross(a,b);
  905.   float d2 = axb.norm2();
  906.   if (d2 < 1.e-7) return false;
  907.   isect = p1 + a * dot(axb, cross((p3-p1),b)) / d2;
  908.   return true;
  909. }
  912. inline Pnt3 &
  913. Pnt3::xform(float m[16])
  914. {
  915.   float x = m[0]*v[0] + m[4]*v[1] + m[8] *v[2] + m[12];
  916.   float y = m[1]*v[0] + m[5]*v[1] + m[9] *v[2] + m[13];
  917.   v[2]    = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14];
  918.   v[0] = x; v[1] = y;
  919.   return *this;
  920. }
  923. inline Pnt3 &
  924. Pnt3::xform(double m[16])
  925. {
  926.   float x = m[0]*v[0] + m[4]*v[1] + m[8] *v[2] + m[12];
  927.   float y = m[1]*v[0] + m[5]*v[1] + m[9] *v[2] + m[13];
  928.   v[2]    = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14];
  929.   v[0] = x; v[1] = y;
  930.   return *this;
  931. }
  934. inline Pnt3 &
  935. Pnt3::xform(float r[3][3], float t[3])
  936. {
  937.   float x = t[0] + r[0][0]*v[0] + r[0][1]*v[1] + r[0][2]*v[2];
  938.   float y = t[1] + r[1][0]*v[0] + r[1][1]*v[1] + r[1][2]*v[2];
  939.   v[2]    = t[2] + r[2][0]*v[0] + r[2][1]*v[1] + r[2][2]*v[2];
  940.   v[0] = x; v[1] = y;
  941.   return *this;
  942. }
  944. inline Pnt3 &
  945. Pnt3::xform(float r[3][3], double t[3])
  946. {
  947.   float x = t[0] + r[0][0]*v[0] + r[0][1]*v[1] + r[0][2]*v[2];
  948.   float y = t[1] + r[1][0]*v[0] + r[1][1]*v[1] + r[1][2]*v[2];
  949.   v[2]    = t[2] + r[2][0]*v[0] + r[2][1]*v[1] + r[2][2]*v[2];
  950.   v[0] = x; v[1] = y;
  951.   return *this;
  952. }
  954. inline Pnt3 &
  955. Pnt3::invxform(float m[16])
  956. {
  957.   float tx = v[0] - m[12];
  958.   float ty = v[1] - m[13];
  959.   float tz = v[2] - m[14];
  960.   v[0] = m[0]*tx + m[1]*ty + m[2]*tz;
  961.   v[1] = m[4]*tx + m[5]*ty + m[6]*tz;
  962.   v[2] = m[8]*tx + m[9]*ty + m[10]*tz;
  963.   return *this;
  964. }
  966. inline Pnt3 &
  967. Pnt3::invxform(double m[16])
  968. {
  969.   double tx = v[0] - m[12];
  970.   double ty = v[1] - m[13];
  971.   double tz = v[2] - m[14];
  972.   v[0] = m[0]*tx + m[1]*ty + m[2]*tz;
  973.   v[1] = m[4]*tx + m[5]*ty + m[6]*tz;
  974.   v[2] = m[8]*tx + m[9]*ty + m[10]*tz;
  975.   return *this;
  976. }
  978. inline Pnt3 &
  979. Pnt3::invxform(float r[3][3], float t[3])
  980. {
  981.   float tx = v[0] - t[0];
  982.   float ty = v[1] - t[1];
  983.   float tz = v[2] - t[2];
  984.   v[0] = r[0][0]*tx + r[1][0]*ty + r[2][0]*tz;
  985.   v[1] = r[0][1]*tx + r[1][1]*ty + r[2][1]*tz;
  986.   v[2] = r[0][2]*tx + r[1][2]*ty + r[2][2]*tz;
  987.   return *this;
  988. }
  990. inline Pnt3 &
  991. Pnt3::invxform(float r[3][3], double t[3])
  992. {
  993.   float tx = v[0] - t[0];
  994.   float ty = v[1] - t[1];
  995.   float tz = v[2] - t[2];
  996.   v[0] = r[0][0]*tx + r[1][0]*ty + r[2][0]*tz;
  997.   v[1] = r[0][1]*tx + r[1][1]*ty + r[2][1]*tz;
  998.   v[2] = r[0][2]*tx + r[1][2]*ty + r[2][2]*tz;
  999.   return *this;
  1000. }
  1002. inline Pnt3 &
  1003. Pnt3::setXformed(const Pnt3 &p, float r[3][3], float t[3])
  1004. {
  1005.   v[0] = r[0][0]*p.v[0] + r[0][1]*p.v[1] + r[0][2]*p.v[2] + t[0];
  1006.   v[1] = r[1][0]*p.v[0] + r[1][1]*p.v[1] + r[1][2]*p.v[2] + t[1];
  1007.   v[2] = r[2][0]*p.v[0] + r[2][1]*p.v[1] + r[2][2]*p.v[2] + t[2];
  1008.   return *this;
  1009. }
  1011. inline Pnt3 &
  1012. Pnt3::setRotated(const Pnt3 &p, float r[3][3])
  1013. {
  1014.   v[0] = r[0][0]*p.v[0] + r[0][1]*p.v[1] + r[0][2]*p.v[2];
  1015.   v[1] = r[1][0]*p.v[0] + r[1][1]*p.v[1] + r[1][2]*p.v[2];
  1016.   v[2] = r[2][0]*p.v[0] + r[2][1]*p.v[1] + r[2][2]*p.v[2];
  1017.   return *this;
  1018. }
  1020. class Vec3 : public Pnt3 {
  1021. public:
  1022.   Vec3(float a=0.0, float b=0.0, float c=0.0) 
  1023.     { v[0] = a, v[1] = b, v[2] = c; normalize(); }
  1024.   Vec3(const Pnt3 &p)
  1025.     { v[0] = p[0]; v[1] = p[1]; v[2] = p[2]; normalize(); }
  1026.   Vec3 &xform(float m[16]); // OpenGL model matrix
  1027.   Vec3 &xform(float r[3][3]);
  1028. };
  1030. inline Vec3 &
  1031. Vec3::xform(float m[16])
  1032. {
  1033.   float x = m[0]*v[0] + m[4]*v[1] + m[8] *v[2];
  1034.   float y = m[1]*v[0] + m[5]*v[1] + m[9] *v[2];
  1035.   v[2]    = m[2]*v[0] + m[6]*v[1] + m[10]*v[2];
  1036.   v[0] = x; v[1] = y;
  1037.   return *this;
  1038. }
  1040. inline Vec3 &
  1041. Vec3::xform(float r[3][3])
  1042. {
  1043.   float x = r[0][0]*v[0] + r[0][1]*v[1] + r[0][2]*v[2];
  1044.   float y = r[1][0]*v[0] + r[1][1]*v[1] + r[1][2]*v[2];
  1045.   v[2]    = r[2][0]*v[0] + r[2][1]*v[1] + r[2][2]*v[2];
  1046.   v[0] = x; v[1] = y;
  1047.   return *this;
  1048. }
  1050. #endif
  1055. #ifdef PNT3_MAIN
  1057. #include <iostream.h>
  1058. #define SHOW(X) cout << #X " = " << X << endl
  1060. void
  1061. main(void)
  1062. {
  1063.   Pnt3 a;
  1064.   Pnt3 b(1,1,1);
  1065.   SHOW(a);
  1066.   SHOW(b);
  1068.   SHOW(dist2_lineseg(a, a, b));
  1069.   SHOW(dist2_lineseg(b, a, b));
  1070.   SHOW(dist2_lineseg(.5*(a+b), a, b));
  1071.   SHOW(dist2_lineseg(a+Pnt3(1,0,0), a, b));
  1072.   SHOW(dist2_lineseg(b+Pnt3(1,0,0), a, b));
  1073.   SHOW(dist2_lineseg(2*b, a, b));
  1074.   SHOW(dist2_lineseg( -b, a, b));
  1076.   Pnt3 c(1,0,0);
  1077.   SHOW(c);
  1078.   
  1079.   SHOW(dist2_tri(a, a,b,c));
  1080.   SHOW(dist2_tri(b, a,b,c));
  1081.   SHOW(dist2_tri(c, a,b,c));
  1082.   SHOW(dist2_tri((a+b+c)/3.0, a,b,c));
  1083.   SHOW(dist2_tri(Pnt3(0,-1,1), a,b,c));
  1084.   SHOW(dist2_tri((a+b+c)/3.0 + 2*Pnt3(0,-1,1), a,b,c));
  1085. }
  1087. #endif