rtcGeometry3D.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2008
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: PUMA: Probablistic Unsupervised Model Acquisition
00011  * file .......: Geometry.cpp
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 07/25/2006
00015  * modified ...: $Date: 2008-12-21 23:41:02 -0800 (Sun, 21 Dec 2008) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 625 $
00018  */
00019 
00020 //== INCLUDES ==================================================================
00021 #include "rtc/rtcGeometry3D.h"
00022 
00023 //== NAMESPACES ================================================================
00024 namespace rtc {
00025 
00026 // dot product
00027 float dot(const Vec3f &a, const Vec3f &b)
00028 {
00029   return a.dot(b);
00030 }
00031 
00032   // cross product
00033 Vec3f cross(const Vec3f &a, const Vec3f &b)
00034 {
00035   return a.cross(b);
00036 }
00037 
00038 // two vectors, a and b, starting from c
00039 Vec3f cross(const Vec3f &a, const Vec3f &b, const Vec3f &c)
00040 {
00041   float a0 = a[0]-c[0];
00042   float a1 = a[1]-c[1];
00043   float a2 = a[2]-c[2];
00044   float b0 = b[0]-c[0];
00045   float b1 = b[1]-c[1];
00046   float b2 = b[2]-c[2];
00047   return Vec3f(a1*b2 - a2*b1, a2*b0 - a0*b2, a0*b1 - a1*b0);
00048 }
00049 
00050 float dist2(const Vec3f &a, const Vec3f &b)
00051 {
00052   float x = a[0]-b[0];
00053   float y = a[1]-b[1];
00054   float z = a[2]-b[2];
00055   return x*x + y*y + z*z;
00056 }
00057 
00058 float dist(const Vec3f &a, const Vec3f &b)
00059 {
00060   return sqrtf(dist2(a,b));
00061 }
00062 
00063 // linear interpolation
00064 Vec3f lerp(float t, const Vec3f &a, const Vec3f &b)
00065 {
00066   float v[3];
00067   float u = 1.0 - t;
00068   v[0]=u*a[0]+t*b[0];
00069   v[1]=u*a[1]+t*b[1];
00070   v[2]=u*a[2]+t*b[2];
00071   return Vec3f(v[0],v[1],v[2]);
00072 }
00073 
00074 // is the point p within the box centered at bc, with radius br?
00075 bool point_within_bounds(const Vec3f &p, const Vec3f &bc, float br)
00076 {
00077   Vec3f tmp((p[0]-bc[0])/br, (p[1]-bc[1])/br, (p[2]-bc[2])/br);
00078   if (tmp[0] >  .5) return false;
00079   if (tmp[0] < -.5) return false;
00080   if (tmp[1] >  .5) return false;
00081   if (tmp[1] < -.5) return false;
00082   if (tmp[2] >  .5) return false;
00083   if (tmp[2] < -.5) return false;
00084   return true;
00085 }
00086 
00087 // is the ball centered at b with radius r
00088 // fully within the box centered at bc, with radius br?
00089 bool ball_within_bounds(const Vec3f &b, float r, const Vec3f &bc, float br)
00090 {
00091   r -= br;
00092   if ((b[0] - bc[0] <= r) ||
00093       (bc[0] - b[0] <= r) ||
00094       (b[1] - bc[1] <= r) ||
00095       (bc[1] - b[1] <= r) ||
00096       (b[2] - bc[2] <= r) ||
00097       (bc[2] - b[2] <= r)) return false;
00098   return true;
00099 }
00100 
00101 
00102 // is the ball centered at b with radius r
00103 // fully within the box centered from min to max?
00104 bool ball_within_bounds(const Vec3f &b, float r, const Vec3f &min, const Vec3f &max)
00105 {
00106   if ((b[0] - min[0] <= r) ||
00107       (max[0] - b[0] <= r) ||
00108       (b[1] - min[1] <= r) ||
00109       (max[1] - b[1] <= r) ||
00110       (b[2] - min[2] <= r) ||
00111       (max[2] - b[2] <= r)) return false;
00112   return true;
00113 }
00114 
00115 
00116 // does the ball centered at b, with radius r,
00117 // intersect the box centered at bc, with radius br?
00118 bool bounds_overlap_ball(const Vec3f &b, float r, const Vec3f &bc, float br)
00119 {
00120   float sum = 0.0, tmp;
00121   if        ((tmp = bc[0]-br - b[0]) > 0.0) {
00122     if (tmp>r) return false; sum += tmp*tmp;
00123   } else if ((tmp = b[0] - (bc[0]+br)) > 0.0) {
00124     if (tmp>r) return false; sum += tmp*tmp;
00125   }
00126   if        ((tmp = bc[1]-br - b[1]) > 0.0) {
00127     if (tmp>r) return false; sum += tmp*tmp;
00128   } else if ((tmp = b[1] - (bc[1]+br)) > 0.0) {
00129     if (tmp>r) return false; sum += tmp*tmp;
00130   }
00131   if        ((tmp = bc[2]-br - b[2]) > 0.0) {
00132     if (tmp>r) return false; sum += tmp*tmp;
00133   } else if ((tmp = b[2] - (bc[2]+br)) > 0.0) {
00134     if (tmp>r) return false; sum += tmp*tmp;
00135   }
00136   return (sum < r*r);
00137 }
00138 
00139 bool bounds_overlap_ball(const Vec3f &b, float r, const Vec3f &min, const Vec3f &max)
00140 {
00141   float sum = 0.0, tmp;
00142   if        (b[0] < min[0]) {
00143     tmp = min[0] - b[0]; if (tmp>r) return false; sum+=tmp*tmp;
00144   } else if (b[0] > max[0]) {
00145     tmp = b[0] - max[0]; if (tmp>r) return false; sum+=tmp*tmp;
00146   }
00147   if        (b[1] < min[1]) {
00148     tmp = min[1] - b[1]; sum+=tmp*tmp;
00149   } else if (b[1] > max[1]) {
00150     tmp = b[1] - max[1]; sum+=tmp*tmp;
00151   }
00152   r *= r;
00153   if (sum > r) return false;
00154   if        (b[2] < min[2]) {
00155     tmp = min[2] - b[2]; sum+=tmp*tmp;
00156   } else if (b[2] > max[2]) {
00157     tmp = b[2] - max[2]; sum+=tmp*tmp;
00158   }
00159   return (sum < r);
00160 }
00161 
00162 // calculate barycentric coordinates of the point p
00163 // (already on the triangle plane) with normal vector n
00164 // and two edge vectors v1 and v2,
00165 // starting from a common vertex t0
00166 void bary_fast(const Vec3f& p, const Vec3f& n, const Vec3f &t0, const Vec3f& v1, const Vec3f& v2, float &b1, float &b2, float &b3)
00167 {
00168   // see bary above
00169   int i = 0;
00170   if (abs(n[1]) > abs(n[0])) i = 1;
00171   if (abs(n[2]) > abs(n[i])) {
00172     // ignore z
00173     float d = 1.0 / (v1[0]*v2[1]-v1[1]*v2[0]);
00174     float x0 = (p[0]-t0[0]);
00175     float x1 = (p[1]-t0[1]);
00176     b1 = (x0*v2[1] - x1*v2[0]) * d;
00177     b2 = (v1[0]*x1 - v1[1]*x0) * d;
00178   } else if (i==0) {
00179     // ignore x
00180     float d = 1.0 / (v1[1]*v2[2]-v1[2]*v2[1]);
00181     float x0 = (p[1]-t0[1]);
00182     float x1 = (p[2]-t0[2]);
00183     b1 = (x0*v2[2] - x1*v2[1]) * d;
00184     b2 = (v1[1]*x1 - v1[2]*x0) * d;
00185   } else {
00186     // ignore y
00187     float d = 1.0 / (v1[2]*v2[0]-v1[0]*v2[2]);
00188     float x0 = (p[2]-t0[2]);
00189     float x1 = (p[0]-t0[0]);
00190     b1 = (x0*v2[0] - x1*v2[2]) * d;
00191     b2 = (v1[2]*x1 - v1[0]*x0) * d;
00192   }
00193   b3 = 1.0 - b1 - b2;
00194 }
00195 
00196 bool closer_on_lineseg(const Vec3f &x, Vec3f &cp, const Vec3f &a, const Vec3f &b, float &d2)
00197 {
00198   Vec3f ba(b[0]-a[0], b[1]-a[1], b[2]-a[2]);
00199   Vec3f xa(x[0]-a[0], x[1]-a[1], x[2]-a[2]);
00200 
00201   float xa_ba = dot(xa,ba);
00202   // if the dot product is negative, the point is closest to a
00203   if (xa_ba < 0.0) {
00204     float nd = dist2(x,a);
00205     if (nd < d2) { cp = a; d2 = nd; return true; }
00206     return false;
00207   }
00208 
00209   // if the dot product is greater than squared segment length,
00210   // the point is closest to b
00211   float fact = xa_ba/ba.normSqr();
00212   if (fact >= 1.0) {
00213     float nd = dist2(x,b);
00214     if (nd < d2) { cp = b; d2 = nd; return true; }      return false;
00215   }
00216 
00217   // take the squared dist x-a, squared dot of x-a to unit b-a,
00218   // use Pythagoras' rule
00219   float nd = xa.normSqr() - xa_ba*fact;
00220   if (nd < d2) {
00221     d2 = nd;
00222     cp[0] = a[0] + fact * ba[0];
00223     cp[1] = a[1] + fact * ba[1];
00224     cp[2] = a[2] + fact * ba[2];
00225     return true;
00226   }
00227   return false;
00228 }
00229 
00230 void distance_point_line(const Vec3f &x, const Vec3f &a, const Vec3f &b, float &d2, Vec3f &cp)
00231 {
00232   Vec3f ba(b[0]-a[0], b[1]-a[1], b[2]-a[2]);
00233   Vec3f xa(x[0]-a[0], x[1]-a[1], x[2]-a[2]);
00234 
00235   float xa_ba = dot(xa,ba);
00236 
00237   // if the dot product is negative, the point is closest to a
00238   if (xa_ba < 0.0) {
00239    d2 = dist2(x,a);
00240    cp = a;
00241    return;
00242   }
00243 
00244   // if the dot product is greater than squared segment length,
00245   // the point is closest to b
00246   float fact = xa_ba/ba.normSqr();
00247   if (fact >= 1.0) {
00248     d2 = dist2(x,b);
00249     cp = b;
00250     return;
00251   }
00252 
00253   // take the squared dist x-a, squared dot of x-a to unit b-a,
00254   // use Pythagoras' rule
00255   d2 = xa.normSqr() - xa_ba*fact;
00256   cp[0] = a[0] + fact * ba[0];
00257   cp[1] = a[1] + fact * ba[1];
00258   cp[2] = a[2] + fact * ba[2];
00259   return;
00260 }
00261 
00262 
00263 void distance_point_tri(const Vec3f &x, const Vec3f &t1, const Vec3f &t2, const Vec3f &t3, float &d2, Vec3f &cp)
00264 {
00265   // calculate the normal and distance from the plane
00266   Vec3f  v1(t2[0]-t1[0], t2[1]-t1[1], t2[2]-t1[2]);
00267   Vec3f  v2(t3[0]-t1[0], t3[1]-t1[1], t3[2]-t1[2]);
00268   Vec3f  n = cross(v1,v2);
00269   float n_inv_mag2 = 1.0/n.normSqr();
00270   float tmp  = (x[0]-t1[0])*n[0] +
00271     (x[1]-t1[1])*n[1] +
00272     (x[2]-t1[2])*n[2];
00273   float distp2 = tmp * tmp * n_inv_mag2;
00274 
00275   // calculate the barycentric coordinates of the point
00276   // (projected onto tri plane) with respect to v123
00277   float b1,b2,b3;
00278   float f = tmp*n_inv_mag2;
00279   Vec3f   pp(x[0]-f*n[0], x[1]-f*n[1], x[2]-f*n[2]);
00280   bary_fast(pp, n, t1,v1,v2, b1,b2,b3);
00281 
00282   // all non-negative, the point is within the triangle
00283   if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
00284     d2 = distp2;
00285     cp = pp;
00286     return;
00287   }
00288 
00289   // look at the signs of the barycentric coordinates
00290   // if there are two negative signs, the positive
00291   // one tells the vertex that's closest
00292   // if there's one negative sign, the opposite edge
00293   // (with endpoints) is closest
00294 
00295   if (b1 < 0.0) {
00296     if (b2 < 0.0) {
00297       d2 = dist2(x,t3); cp = t3;
00298     } else if (b3 < 0.0) {
00299       d2 = dist2(x,t2); cp = t2;
00300     } else {
00301       distance_point_line(x, t2, t3, d2, cp);
00302     }
00303   } else if (b2 < 0.0) {
00304     if (b3 < 0.0) {
00305       d2 = dist2(x,t1); cp = t1;
00306     } else  {
00307      distance_point_line(x, t1, t3, d2, cp);
00308     }
00309   } else {
00310     distance_point_line(x, t1, t2, d2, cp);
00311   }
00312   return;
00313 }
00314 
00315 bool closer_on_tri(const Vec3f &x, Vec3f &cp, const Vec3f &t1, const Vec3f &t2, const Vec3f &t3, float &d2)
00316 {
00317   // calculate the normal and distance from the plane
00318   Vec3f  v1(t2[0]-t1[0], t2[1]-t1[1], t2[2]-t1[2]);
00319   Vec3f  v2(t3[0]-t1[0], t3[1]-t1[1], t3[2]-t1[2]);
00320   Vec3f  n = cross(v1,v2);
00321   float n_inv_mag2 = 1.0/n.normSqr();
00322   float tmp  = (x[0]-t1[0])*n[0] +
00323               (x[1]-t1[1])*n[1] +
00324               (x[2]-t1[2])*n[2];
00325   float distp2 = tmp * tmp * n_inv_mag2;
00326   if (distp2 >= d2) return false;
00327 
00328   // calculate the barycentric coordinates of the point
00329   // (projected onto tri plane) with respect to v123
00330   float b1,b2,b3;
00331   float f = tmp*n_inv_mag2;
00332   Vec3f   pp(x[0]-f*n[0], x[1]-f*n[1], x[2]-f*n[2]);
00333   bary_fast(pp, n, t1,v1,v2, b1,b2,b3);
00334 
00335   // all non-negative, the point is within the triangle
00336   if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
00337     d2 = distp2;
00338     cp = pp;
00339     return true;
00340   }
00341 
00342   // look at the signs of the barycentric coordinates
00343   // if there are two negative signs, the positive
00344   // one tells the vertex that's closest
00345   // if there's one negative sign, the opposite edge
00346   // (with endpoints) is closest
00347 
00348   if (b1 < 0.0) {
00349     if (b2 < 0.0) {
00350       float nd = dist2(x,t3);
00351       if (nd < d2) { d2 = nd; cp = t3; return true; }
00352       else         { return false; }
00353     } else if (b3 < 0.0) {
00354       float nd = dist2(x,t2);
00355       if (nd < d2) { d2 = nd; cp = t2; return true; }
00356       else         { return false; }
00357     } else return closer_on_lineseg(x, cp, t2,t3, d2);
00358   } else if (b2 < 0.0) {
00359     if (b3 < 0.0) {
00360       float nd = dist2(x,t1);
00361       if (nd < d2) { d2 = nd; cp = t1; return true; }
00362       else         { return false; }
00363     } else return closer_on_lineseg(x, cp, t1,t3, d2);
00364   } else   return closer_on_lineseg(x, cp, t1,t2, d2);
00365 }
00366 
00367 // calculate the intersection of a line going through p
00368 // to direction dir with a plane spanned by t1,t2,t3
00369 // (modified from Graphics Gems, p.299)
00370 bool
00371 line_plane_X(const Vec3f& p, const Vec3f& dir,
00372 const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00373 Vec3f &x, float &dist)
00374 {
00375   // note: normal doesn't need to be unit vector
00376   Vec3f nrm = cross(t1,t2,t3);
00377   float tmp = dot(nrm,dir);
00378   if (tmp == 0.0) {
00379 //    std::cerr << "Cannot intersect plane with a parallel line" << std::endl;
00380     return false;
00381   }
00382   // d  = -dot(nrm,t1)
00383   // t  = - (d + dot(p,nrm))/dot(dir,nrm)
00384   // is = p + dir * t
00385   x = dir;
00386   dist = (dot(nrm,t1)-dot(nrm,p))/tmp;
00387   x *= dist;
00388   x += p;
00389   if (dist < 0.0) dist = -dist;
00390   return true;
00391 }
00392 
00393 bool
00394 line_plane_X(const Vec3f& p, const Vec3f& dir,
00395 const Vec3f& nrm, float d, Vec3f &x, float &dist)
00396 {
00397   float tmp = dot(nrm,dir);
00398   if (tmp == 0.0) {
00399     std::cerr << "Cannot intersect plane with a parallel line" << std::endl;
00400     return false;
00401   }
00402   x = dir;
00403   dist = -(d+dot(nrm,p))/tmp;
00404   x *= dist;
00405   x += p;
00406   if (dist < 0.0) dist = -dist;
00407   return true;
00408 }
00409 
00410 // calculate barycentric coordinates of the point p
00411 // on triangle t1 t2 t3
00412 void
00413 bary(const Vec3f& p,
00414 const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00415 float &b1, float &b2, float &b3)
00416 {
00417   // figure out the plane onto which to project the vertices
00418   // by calculating a cross product and finding its largest dimension
00419   // then use Cramer's rule to calculate two of the
00420   // barycentric coordinates
00421   // e.g., if the z coordinate is ignored, and v1 = t1-t3, v2 = t2-t3
00422   // b1 = det[x[0] v2[0]; x[1] v2[1]] / det[v1[0] v2[0]; v1[1] v2[1]]
00423   // b2 = det[v1[0] x[0]; v1[1] x[1]] / det[v1[0] v2[0]; v1[1] v2[1]]
00424   float v10 = t1[0]-t3[0];
00425   float v11 = t1[1]-t3[1];
00426   float v12 = t1[2]-t3[2];
00427   float v20 = t2[0]-t3[0];
00428   float v21 = t2[1]-t3[1];
00429   float v22 = t2[2]-t3[2];
00430   float c[2];
00431   c[0] = fabs(v11*v22 - v12*v21);
00432   c[1] = fabs(v12*v20 - v10*v22);
00433   int i = 0;
00434   if (c[1] > c[0]) i = 1;
00435   if (fabs(v10*v21 - v11*v20) > c[i]) {
00436     // ignore z
00437     float d = 1.0f / (v10*v21-v11*v20);
00438     float x0 = (p[0]-t3[0]);
00439     float x1 = (p[1]-t3[1]);
00440     b1 = (x0*v21 - x1*v20) * d;
00441     b2 = (v10*x1 - v11*x0) * d;
00442   } else if (i==0) {
00443     // ignore x
00444     float d = 1.0f / (v11*v22-v12*v21);
00445     float x0 = (p[1]-t3[1]);
00446     float x1 = (p[2]-t3[2]);
00447     b1 = (x0*v22 - x1*v21) * d;
00448     b2 = (v11*x1 - v12*x0) * d;
00449   } else {
00450     // ignore y
00451     float d = 1.0f / (v12*v20-v10*v22);
00452     float x0 = (p[2]-t3[2]);
00453     float x1 = (p[0]-t3[0]);
00454     b1 = (x0*v20 - x1*v22) * d;
00455     b2 = (v12*x1 - v10*x0) * d;
00456   }
00457   b3 = 1.0f - b1 - b2;
00458 }
00459 
00460 // calculate barycentric coordinates for the intersection of
00461 // a line starting from p, going to direction dir, and the plane
00462 // of the triangle t1 t2 t3
00463 bool
00464 bary(const Vec3f& p, const Vec3f& dir,
00465 const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00466 float &b1, float &b2, float &b3)
00467 {
00468   Vec3f x; float d;
00469   if(!line_plane_X(p, dir, t1, t2, t3, x, d))
00470     return false;
00471   bary(x, t1,t2,t3, b1,b2,b3);
00472 
00473   return true;
00474 }
00475 
00476 // calculate the intersection of a line starting from p,
00477 // going to direction dir, and the triangle t1 t2 t3
00478 bool
00479 line_tri_X(const Vec3f& p, const Vec3f& dir,
00480 const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00481 Vec3f& x, float& d)
00482 {
00483   float b1,b2,b3;
00484   Vec3f x_temp; float d_temp;
00485   if(!line_plane_X(p, dir, t1, t2, t3, x_temp, d_temp))
00486     return false;
00487 
00488   bary(x_temp, t1,t2,t3, b1,b2,b3);
00489   // all non-negative, the point is within the triangle
00490   if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
00491     x=x_temp;
00492     d=d_temp;
00493     return true;
00494   }
00495   return false;
00496 }
00497 bool closer_on_line(const Vec3f &x, const Vec3f &a, const Vec3f &b, float &d2, Vec3f &cp)
00498 {
00499   Vec3f ba(b[0]-a[0], b[1]-a[1], b[2]-a[2]);
00500   Vec3f xa(x[0]-a[0], x[1]-a[1], x[2]-a[2]);
00501 
00502   float xa_ba = xa.dot(ba);
00503   // if the dot product is negative, the point is closest to a
00504   if (xa_ba < 0.0) {
00505     float nd = dist2(x, a);
00506     if (nd < d2) {
00507       cp = a;
00508       d2 = nd;
00509       return true;
00510     }
00511     return false;
00512   }
00513 
00514   // if the dot product is greater than squared segment length,
00515   // the point is closest to b
00516   float fact = xa_ba/ba.normSqr();
00517   if (fact >= 1.0) {
00518     float nd = dist2(x,b);
00519     if (nd < d2) {
00520       cp = b;
00521       d2 = nd;
00522       return true;
00523     }
00524     return false;
00525   }
00526 
00527   // take the squared dist x-a, squared dot of x-a to unit b-a,
00528   // use Pythagoras' rule
00529   float nd = xa.normSqr() - xa_ba*fact;
00530   if (nd < d2) {
00531     d2 = nd;
00532     cp[0] = a[0] + fact * ba[0];
00533     cp[1] = a[1] + fact * ba[1];
00534     cp[2] = a[2] + fact * ba[2];
00535     return true;
00536   }
00537   return false;
00538 }
00539 
00540 void dist_to_line(const Vec3f &x, const Vec3f &a, const Vec3f &b, float &d, Vec3f &cp)
00541 {
00542   Vec3f ba(b[0]-a[0], b[1]-a[1],b[2]-a[2]);
00543   Vec3f xa(x[0]-a[0], x[1]-a[1],x[2]-a[2]);
00544 
00545   float xa_ba = xa.dot(ba);
00546   // if the dot product is negative, the point is closest to a
00547   if (xa_ba < 0.0) {
00548     float nd = dist(x,a);
00549     cp = a;
00550     d = nd;
00551     return;
00552   }
00553 
00554   // if the dot product is greater than squared segment length,
00555   // the point is closest to b
00556   float fact = xa_ba/ba.normSqr();
00557   if (fact >= 1.0) {
00558     float nd = dist(x,b);
00559     cp = b;
00560     d = nd;
00561     return;
00562   }
00563 
00564   // take the squared dist x-a, squared dot of x-a to unit b-a,
00565   // use Pythagoras' rule
00566   float nd = xa.normSqr() - xa_ba*fact;
00567   d = sqrt(nd);
00568   cp[0] = a[0] + fact * ba[0];
00569   cp[1] = a[1] + fact * ba[1];
00570   cp[2] = a[2] + fact * ba[2];
00571 }
00572 
00573 float dist_to_line(const Vec3f &x, const Vec3f &a, const Vec3f &b)
00574 {
00575   Vec3f ba(b[0]-a[0], b[1]-a[1], b[2]-a[2]);
00576   Vec3f xa(x[0]-a[0], x[1]-a[1], x[2]-a[2]);
00577 
00578   float xa_ba = xa.dot(ba);
00579   // if the dot product is negative, the point is closest to a
00580   if (xa_ba < 0.0) {
00581     return dist(x,a);
00582   }
00583 
00584   // if the dot product is greater than squared segment length,
00585   // the point is closest to b
00586   float fact = xa_ba/ba.normSqr();
00587   if (fact >= 1.0) {
00588     return dist(x,b);
00589   }
00590 
00591   // take the squared dist x-a, squared dot of x-a to unit b-a,
00592   // use Pythagoras' rule
00593   return sqrt(xa.normSqr() - xa_ba*fact);
00594 }
00595 //==============================================================================
00596 } // namespace puma
00597 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53