From: abn Date: Mon, 24 Apr 2017 08:39:22 +0000 (+0200) Subject: Bug fix: distanceToPoint(s) - fix with a new algo. More efficient too. X-Git-Url: http://git.salome-platform.org/gitweb/?a=commitdiff_plain;h=77a8ead0944ffc15518c24d1ccef0daea37fa849;p=tools%2Fmedcoupling.git Bug fix: distanceToPoint(s) - fix with a new algo. More efficient too. --- diff --git a/src/INTERP_KERNEL/BBTreeDst.txx b/src/INTERP_KERNEL/BBTreeDst.txx index 60c3084ce..9497070aa 100644 --- a/src/INTERP_KERNEL/BBTreeDst.txx +++ b/src/INTERP_KERNEL/BBTreeDst.txx @@ -134,6 +134,10 @@ public: } } + /** Get the minimal (square) distance between a point and all the available bounding boxes in the tree. + The (square) distance to a bbox is the true geometric distance between the point and a face + (or an edge, or a corner) of the bbox. + */ void getMinDistanceOfMax(const double *pt, double& minOfMaxDstsSq) const { if(_terminal) diff --git a/src/INTERP_KERNEL/VolSurfUser.cxx b/src/INTERP_KERNEL/VolSurfUser.cxx index 2d06cdde0..5877f0e31 100644 --- a/src/INTERP_KERNEL/VolSurfUser.cxx +++ b/src/INTERP_KERNEL/VolSurfUser.cxx @@ -20,6 +20,7 @@ #include "VolSurfUser.hxx" #include "InterpKernelAutoPtr.hxx" +#include "InterpolationUtils.hxx" #include #include @@ -63,29 +64,152 @@ namespace INTERP_KERNEL return (x-pt[0])*(x-pt[0])+(y-pt[1])*(y-pt[1]); } + /** + * See http://geomalgorithms.com/a02-_lines.html#Distance-to-Ray-or-Segment + */ + double DistanceFromPtToSegInSpaceDim3(const double *pt, const double *pt0Seg2, const double *pt1Seg2) + { + double v[3], w[3]; + for(int i=0; i < 3; i++) { + v[i]=pt1Seg2[i]-pt0Seg2[i]; + w[i] = pt[i] - pt0Seg2[i]; + } + + double c1 = dotprod<3>(w,v); + if ( c1 <= 0 ) + return norm<3>(w); + double c2 = dotprod<3>(v,v); + if ( c2 <= c1 ) + { + for(int i=0; i < 3; i++) + w[i] = pt[i] - pt1Seg2[i]; + return norm<3>(w); + } + double b = c1 / c2; + for(int i=0; i < 3; i++) + w[i] = pt0Seg2[i] + b * v[i] - pt[i]; + return norm<3>(w); + } + + /** + Helper for DistanceFromPtToTriInSpaceDim3 + */ + inline double _HelperDistancePtToTri3D_1(const double aXX, const double bX, const double c) + { + if (bX >= 0) + return c; + if (-bX >= aXX) + return aXX + 2*bX + c; + return bX*(-bX / aXX) + c; + } + + /** + Helper for DistanceFromPtToTriInSpaceDim3 + */ + inline double _HelperDistancePtToTri3D_2(const double a01, const double aXX, const double aYY, + const double bX, const double bY, const double c) + { + double tmp0 = a01 + bX, tmp1 = aXX + bY; + if (tmp1 > tmp0) { + double numer = tmp1 - tmp0, denom = aXX - 2*a01 + aYY; + if (numer >= denom) + return aXX + 2*bX + c; + else { + double s, t; + s = numer / denom; t = 1 - s; + return s*(aXX*s + a01*t + 2*bX) + t*(a01*s + aYY*t + 2*bY) + c; + } + } + else + { + if (tmp1 <= 0) return aYY + 2*bY + c; + else { + if (bY >= 0) return c; + else return bY*(-bY / aYY) + c; + } + } + } + + /** + * From https://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf + */ double DistanceFromPtToTriInSpaceDim3(const double *pt, const double *pt0Tri3, const double *pt1Tri3, const double *pt2Tri3) { - double matrix[12]; - if(!ComputeRotTranslationMatrixToPut3PointsOnOXY(pt0Tri3,pt1Tri3,pt2Tri3,matrix)) - return std::numeric_limits::max(); - double xy0[2],xy1[2],xy2[2],xy[2]; xy0[0]=0.; xy0[1]=0.; - xy1[0]=matrix[0]*pt1Tri3[0]+matrix[1]*pt1Tri3[1]+matrix[2]*pt1Tri3[2]+matrix[3]; xy1[1]=0.; - xy2[0]=matrix[0]*pt2Tri3[0]+matrix[1]*pt2Tri3[1]+matrix[2]*pt2Tri3[2]+matrix[3]; - xy2[1]=matrix[4]*pt2Tri3[0]+matrix[5]*pt2Tri3[1]+matrix[6]*pt2Tri3[2]+matrix[7]; - xy[0]=matrix[0]*pt[0]+matrix[1]*pt[1]+matrix[2]*pt[2]+matrix[3]; - xy[1]=matrix[4]*pt[0]+matrix[5]*pt[1]+matrix[6]*pt[2]+matrix[7]; - double z=matrix[8]*pt[0]+matrix[9]*pt[1]+matrix[10]*pt[2]+matrix[11]; - double ret=std::numeric_limits::max(); - std::size_t nbOfHint=0; - if(xy[0]>0. && xy[0](edge0, edge0), a01=dotprod<3>(edge0,edge1), a11=dotprod<3>(edge1,edge1); + double b0=dotprod<3>(diff, edge0), b1=dotprod<3>(diff, edge1), c=dotprod<3>(diff, diff); + double det = fabs(a00*a11 - a01*a01); + double s = a01*b1 - a11*b0, t = a01*b0 - a00*b1; + double sDist; + + if (s + t <= det) + { + if (s < 0) { + if (t < 0) { // region 4 + if (b0 < 0) { + if (-b0 >= a00) sDist = a00 + 2*b0 + c; + else sDist = b0*(-b0 / a00) + c; + } + else + sDist = _HelperDistancePtToTri3D_1(a11, b1, c); + } + else // region 3 + sDist = _HelperDistancePtToTri3D_1(a11, b1, c); + } + else { + if (t < 0) // region 5 + sDist = _HelperDistancePtToTri3D_1(a00, b0, c); + else // region 0 + { + // minimum at interior point + if (fabs(det) < 1.0e-12) + { + // points are colinear (degenerated triangle) + // => Compute distance between segments + double distance = std::min(DistanceFromPtToSegInSpaceDim3(pt, pt0Tri3, pt1Tri3), + DistanceFromPtToSegInSpaceDim3(pt, pt1Tri3, pt2Tri3)); + return distance; + } + + // else we can divide by non-zero + double invDet = 1 / det; + s *= invDet; t *= invDet; + sDist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; + } + } + } + else // s+t > det + { + if (s < 0.0) // region 2 + sDist = _HelperDistancePtToTri3D_2(a01, a00, a11, b0, b1, c); + else { + if (t < 0.0) // region 6 + sDist = _HelperDistancePtToTri3D_2(a01, a11, a00, b1, b0, c); + else { // region 1 + double numer = a11 + b1 - a01 - b0; + if (numer <= 0.0) + sDist = a11 + 2*b1 + c; + else { + double denom = a00 - 2*a01 + a11; + if (numer >= denom) + sDist = a00 + 2*b0 + c; + else { + s = numer / denom; t = 1 - s; + sDist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; + } + } + } + } + } + // Account for numerical round-off error. + if (sDist < 0) + sDist = 0.0; + + return sqrt(sDist); } double DistanceFromPtToPolygonInSpaceDim3(const double *pt, const int *connOfPolygonBg, const int *connOfPolygonEnd, const double *coords) diff --git a/src/MEDCoupling_Swig/MEDCouplingBasicsTest4.py b/src/MEDCoupling_Swig/MEDCouplingBasicsTest4.py index 5bb897cea..1d7fb6134 100644 --- a/src/MEDCoupling_Swig/MEDCouplingBasicsTest4.py +++ b/src/MEDCoupling_Swig/MEDCouplingBasicsTest4.py @@ -1963,10 +1963,10 @@ class MEDCouplingBasicsTest4(unittest.TestCase): m.insertNextCell(NORM_TRI3,[0,1,2]) a,b=m.distanceToPoint([-0.335,2.27,1.21]) self.assertEqual(0,b) - self.assertAlmostEqual(0.022360988100374124,a,14); + self.assertAlmostEqual(0.0223609881003,a,12); a,b=m.distanceToPoint(DataArrayDouble([-0.335,2.27,1.21],1,3)) self.assertEqual(0,b) - self.assertAlmostEqual(0.022360988100374124,a,14); + self.assertAlmostEqual(0.0223609881003,a,12); a,b=coords.distanceToTuple([-0.335,2.27,1.21]) self.assertAlmostEqual(5.243302871282566,a,14) self.assertEqual(0,b) @@ -2005,6 +2005,27 @@ class MEDCouplingBasicsTest4(unittest.TestCase): self.assertAlmostEqual(0.07071067811865482,a,14) ; self.assertEqual(1,b) # self.assertEqual(2,c) pass + def testSwig2UMeshDistanceToMesh2(self): + mesh = MEDCouplingUMesh('Solid_3', 2) + coo = DataArrayDouble([(99.75,-1.42109e-14,102.75),(99.75,200,102.75),(2.5,0,200),(2.5,200,200),(197,0,200),(197,200,200)]) + mesh.setCoords(coo) + c = DataArrayInt([3, 4, 0, 1, 3, 4, 1, 5, 3, 1, 0, 3, 3, 3, 0, 2]) + cI = DataArrayInt([0, 4, 8, 12, 16]) + mesh.setConnectivity(c, cI) + mesh.checkConsistency() + pt = [125.0, 175.0, 175.0] + # Values computed from GEOM: + exp1, exp2, exp3, exp4 = 54.0633707597, 33.2340187158, 68.9429111657, 99.5221476482 + d1, _ = mesh[0].distanceToPoint(pt) + d2, _ = mesh[1].distanceToPoint(pt) + d3, _ = mesh[2].distanceToPoint(pt) + d4, _ = mesh[3].distanceToPoint(pt) + self.assertAlmostEqual(exp1,d1,10) + self.assertAlmostEqual(exp2,d2,10) + self.assertAlmostEqual(exp3,d3,10) + self.assertAlmostEqual(exp4,d4,10) + pass + def testSwig2NonRegressionPartitionBySpreadZone1(self): m=MEDCouplingCMesh() arr=DataArrayDouble(6) ; arr.iota(0.)