00001 #include "DistFuncs.h"
00002 #if 1
00003 std::ostream &operator<<(std::ostream &ost, const Point& p)
00004 {
00005 ost << "(" << p.x << ", " << p.y << ", " << p.z << ")";
00006 return ost;
00007 }
00008
00009 #endif
00010
00024 inline float SegSegDist(const Point& u0, const Point& u,
00025 const Point& v0, const Point& v,
00026 Point& cp0, Point& cp1)
00027 {
00028 Point w = u0 - v0;
00029 float a = u|u;
00030 float b = u|v;
00031 float c = v|v;
00032 float d = u|w;
00033 float e = v|w;
00034 float D = a*c - b*b;
00035 float sc, sN, sD = D;
00036 float tc, tN, tD = D;
00037
00038
00039 #define EPS 1e-8
00040 if (D < EPS) {
00041 sN = 0.0;
00042 sD = 1.0;
00043 tN = e;
00044 tD = c;
00045 }
00046 else {
00047 sN = (b*e - c*d);
00048 tN = (a*e - b*d);
00049 if (sN < 0.0) {
00050 sN = 0.0;
00051 tN = e;
00052 tD = c;
00053 }
00054 else if (sN > sD) {
00055 sN = sD;
00056 tN = e + b;
00057 tD = c;
00058 }
00059 }
00060
00061 if (tN < 0.0) {
00062 tN = 0.0;
00063
00064 if (-d < 0.0)
00065 sN = 0.0;
00066 else if (-d > a)
00067 sN = sD;
00068 else {
00069 sN = -d;
00070 sD = a;
00071 }
00072 }
00073 else if (tN > tD) {
00074 tN = tD;
00075
00076 if ((-d + b) < 0.0)
00077 sN = 0;
00078 else if ((-d + b) > a)
00079 sN = sD;
00080 else {
00081 sN = (-d + b);
00082 sD = a;
00083 }
00084 }
00085
00086 sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
00087 tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
00088
00089 cp0 = u0 + sc * u;
00090 cp1 = v0 + tc * v;
00091
00092
00093 Point dP = cp0 - cp1;
00094
00095 return dP.Magnitude();
00096 }
00097
00105 inline float PointPlaneDist(const Point& P, const Point& pointOnPlane, const Point& n,
00106 Point& cp)
00107 {
00108 Point v = P - pointOnPlane;
00109 float l = v|n;
00110 cp = P-l*n;
00111 return l;
00112 }
00113
00114
00115 float PointSegDist(const Point& P, const Point& u0, const Point& u1)
00116 {
00117 Point v = P - u0;
00118 Point dir = u1 - u0;
00119 float l = dir.Magnitude();
00120 dir /= l;
00121
00122 float x = v|dir;
00123 if (x < 0){
00124 return v.Magnitude();
00125 }else if (x > l){
00126 Point dv = P - u1;
00127 return dv.Magnitude();
00128 }else{
00129 return sqrtf(v.SquareMagnitude() - x*x);
00130 }
00131 }
00132
00141 inline bool PointFaceAppTest(const Point& P, const Point** vertices,
00142 const Point* edges, const Point& n)
00143 {
00144 Point v, outer;
00145 for (unsigned int i=0; i<3; i++){
00146 v = P - *vertices[i];
00147 outer = edges[i]^n;
00148 if ((v|outer)>0) return false;
00149 }
00150 return true;
00151 }
00152
00153
00154 float TriTriDist(const Point& U0, const Point& U1, const Point& U2,
00155 const Point& V0, const Point& V1, const Point& V2,
00156 Point& cp0, Point& cp1)
00157 {
00158 const Point* uvertices[] = {&U0, &U1, &U2};
00159 const Point* vvertices[] = {&V0, &V1, &V2};
00160 float min_d, d;
00161 Point p0, p1, n, vec;
00162 bool overlap = true;
00163
00164
00165 Point uedges[3], vedges[3];
00166 for (unsigned int i=0; i<3; i++){
00167 uedges[i] = *uvertices[(i+1)%3] - *uvertices[i];
00168 vedges[i] = *vvertices[(i+1)%3] - *vvertices[i];
00169 }
00170
00171
00172 cp0 = U0;
00173 cp1 = V0;
00174 min_d = (cp0-V0).Magnitude();
00175
00176
00177 for (unsigned int i=0; i<3; i++){
00178 for (unsigned int j=0; j<3; j++){
00179 d = SegSegDist(*uvertices[i], uedges[i],
00180 *vvertices[j], vedges[j],
00181 p0, p1);
00182 n = p0 - p1;
00183 if (d <= min_d){
00184 min_d = d;
00185 cp0 = p0;
00186 cp1 = p1;
00187
00188 vec = *uvertices[(i+2)%3] - cp0;
00189 float u = (vec|n)/min_d;
00190 vec = *vvertices[(j+2)%3] - cp1;
00191 float v = (vec|n)/min_d;
00192
00193 if (u>=0 && v<=0) return min_d;
00194
00195 if (u > 0) u = 0;
00196 if (v < 0) v = 0;
00197 if ((n.Magnitude() + u - v) > 0){
00198 overlap = false;
00199 }
00200 }
00201 }
00202 }
00203
00204 Point un = uedges[0]^uedges[1];
00205 un.Normalize();
00206 Point vn = vedges[0]^vedges[1];
00207 vn.Normalize();
00208
00209
00210
00211 float ds[3];
00212 int rank;
00213
00214 rank = -1;
00215 for (int i=0; i<3; i++){
00216 vec = *uvertices[i] - *vvertices[0];
00217 ds[i] = vec|vn;
00218 }
00219 if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){
00220
00221 rank = ds[0] > ds[1] ? 1 : 0;
00222 rank = ds[rank] > ds[2] ? 2 : rank;
00223 }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){
00224
00225 rank = ds[0] > ds[1] ? 0 : 1;
00226 rank = ds[rank] > ds[2] ? rank : 2;
00227 }
00228 if (rank >=0){
00229 overlap = false;
00230 if (PointFaceAppTest(*uvertices[rank], vvertices, vedges, vn)){
00231 min_d = fabsf(PointPlaneDist(*uvertices[rank], *vvertices[0], vn, p1));
00232 cp0 = *uvertices[rank];
00233 cp1 = p1;
00234 return min_d;
00235 }
00236 }
00237
00238
00239 rank = -1;
00240 for (int i=0; i<3; i++){
00241 vec = *vvertices[i] - *uvertices[0];
00242 ds[i] = vec|un;
00243 }
00244 if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){
00245
00246 rank = ds[0] > ds[1] ? 1 : 0;
00247 rank = ds[rank] > ds[2] ? 2 : rank;
00248 }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){
00249
00250 rank = ds[0] > ds[1] ? 0 : 1;
00251 rank = ds[rank] > ds[2] ? rank : 2;
00252 }
00253 if (rank >= 0){
00254 overlap = false;
00255 if (PointFaceAppTest(*vvertices[rank], uvertices, uedges, un)){
00256 min_d = fabsf(PointPlaneDist(*vvertices[rank], *uvertices[0], un, p0));
00257 cp0 = p0;
00258 cp1 = *vvertices[rank];
00259 return min_d;
00260 }
00261 }
00262
00263 if (overlap){
00264 return 0;
00265 }else{
00266 return min_d;
00267 }
00268 }
00269
00270
00271 float SegSegDist(const Point& u0, const Point& u1,
00272 const Point& v0, const Point& v1)
00273 {
00274 Point cp0, cp1;
00275 return SegSegDist(u0, u1-u0, v0, v1-v0, cp0, cp1);
00276 }
00277
00278 #if 0
00279
00280 int main()
00281 {
00282 Point p0(0,0,0), p1(2,0,0), p2(1, 1, -1), p3(1, 1, 1);
00283 Point cp1, cp2, n;
00284 float d;
00285
00286 d= SegSegDist(p0, p1, p2, p3, cp1, cp2);
00287 std::cout << "test1 : d = " << d << ", cp1 = " << cp1
00288 << ", cp2 = " << cp2 << std::endl;
00289
00290 Point p4(0,1,0), p5(2,1,0);
00291 d= SegSegDist(p0, p1, p4, p5, cp1, cp2);
00292 std::cout << "test2 : d = " << d << ", cp1 = " << cp1
00293 << ", cp2 = " << cp2 << std::endl;
00294
00295 d= SegSegDist(p0, p1, p0, p1, cp1, cp2);
00296 std::cout << "test3 : d = " << d << ", cp1 = " << cp1
00297 << ", cp2 = " << cp2 << std::endl;
00298
00299 Point p6(0, 2, 0), p7(-2, 1, 0);
00300 d = TriTriDist(p0, p1, p5, p4, p6, p7, cp1, cp2);
00301 std::cout << "test4 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
00302 << std::endl;
00303
00304 Point p8(3, -1, 1), p9(3, 2, 1), p10(-3, -1, 1);
00305 d = TriTriDist(p0, p1, p5, p8, p9, p10, cp1, cp2);
00306 std::cout << "test5 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
00307 << std::endl;
00308
00309 d = TriTriDist(p0, p1, p2, p8, p9, p10, cp1, cp2);
00310 std::cout << "test6 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
00311 << std::endl;
00312 std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)"
00313 << std::endl;
00314
00315 d = TriTriDist(p2, p0, p1, p8, p9, p10, cp1, cp2);
00316 std::cout << "test7 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
00317 << std::endl;
00318 std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)"
00319 << std::endl;
00320
00321 d = TriTriDist(p1, p2, p0, p8, p9, p10, cp1, cp2);
00322 std::cout << "test8 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2
00323 << std::endl;
00324 std::cout << "answer: d = 1, cp1 = (2, 0, 0), cp2 = (2, 0, 1)"
00325 << std::endl;
00326
00327 {
00328 Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0),
00329 pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1);
00330 d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2);
00331 std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = "
00332 << cp2 << std::endl;
00333 std::cout << "answer: d = 0.0" << std::endl;
00334 }
00335
00336 {
00337 Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0),
00338 pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1);
00339 d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2);
00340 std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = "
00341 << cp2 << std::endl;
00342 std::cout << "answer: d = 0.0" << std::endl;
00343 }
00344
00345 {
00346 Point p0(0.1, 0.4, 0), p1(-0.1, 0.4, 0), p2(0.1, -0.4, 0);
00347 Point p3(0.05, 0.45, -0.15), p4(0.05, 0.45, 0.05), p5(0.05, -0.35, -0.15);
00348 d = TriTriDist(p0, p1, p2, p3, p4, p5, cp1, cp2);
00349 std::cout << "test10 : d = " << d << ", cp1 = " << cp1 << ", cp2 = "
00350 << cp2 << std::endl;
00351 std::cout << "answer: d = 0.0" << std::endl;
00352 }
00353 return 0;
00354 }
00355 #endif