Closest Point Find the Closest Point on a mesh

- Main Page
- Classes
- Files

- File List

src/ClosestPoint.cpp

    00001 #include "ClosestPoint.h"
    00002 #include <utility>
    00003 #include <set>
    00004 #include <algorithm>
    00005 #include <time.h>
    00006
    00007 using namespace std;
    00008
    00009 /*Comparator to sort the pair of Closest Points by distance*/
    00010 bool comp(const std::pair<float,Vec3f>& firstElem,
    00011                 const std::pair<float,Vec3f>& secondElem){
    00012     return firstElem.first < secondElem.first;
    00013 }
    00014
    00015 /*T*/
    00016 Vec3f ClosestPoint::operator () (const Vec3f &queryPoint, float maxRadius){
    00017     /* Closest Point on Mesh, can either be a vertex, point on edge,
    00018      or a point in the interior of a trianlge.
    00019      Returns: min(dVertex,dEdgePoint, dFacePoint)
    00020     */
    00021     cout<< "Query Point ";
    00022     cout << queryPoint.x <<" "<< queryPoint.y << " "<< queryPoint.z;
    00023     cout <<", Max Radius " << maxRadius<<endl;
    00024
    00025     vector< pair <float,Vec3f> > closestPoints;
    00026
    00027     closestPoints.push_back( minVertexDistance(queryPoint,maxRadius));
    00028     closestPoints.push_back( minEdgeDistance(queryPoint,maxRadius));
    00029     closestPoints.push_back( minFaceDistance(queryPoint,maxRadius));
    00030
    00031     std::sort(closestPoints.begin(), closestPoints.end(),comp);
    00032     Vec3f result = closestPoints[0].second;
    00033
    00034     cout <<"-----------------------------------------------------------------"<<endl;
    00035     if (result == NULL)
    00036         cout<< "No Closest Point on mesh found within given maxRadius"<<endl;
    00037     else
    00038         cout <<"CLOSEST POINT ON MESH: "<< result.x<< " "<< result.y<< " "<< result.z<<endl;
    00039     cout <<"-----------------------------------------------------------------"<<endl;
    00040     cout<<endl;
    00041     return result;
    00042 }
    00043
    00044
    00045 pair<float,Vec3f> ClosestPoint::minFaceDistance(const Vec3f &point, float maxRadius){
    00046
    00047     double dmin = INFINITY;
    00048     Vec3f min_face = NULL;
    00049     int faceID;
    00050     std::vector<Face> faces_in_sphere;
    00051     faces_in_sphere = mesh.get_faceList();
    00052
    00053     for(int i = 0; i<faces_in_sphere.size(); i++){
    00054         Vec3f v0 = faces_in_sphere[i].v1;
    00055         Vec3f v1 = faces_in_sphere[i].v2;
    00056         Vec3f v2 = faces_in_sphere[i].v3;
    00057
    00058         Vec3f d  = (v0 - point);
    00059         Vec3f e0 = v1 - v0;
    00060         Vec3f e1 = v2 - v0;
    00061
    00062         e0.normalize();
    00063         e1.normalize();
    00064
    00065         Vec3f a = e0.dot(e0);
    00066         Vec3f b = e0.dot(e1);
    00067         Vec3f c = e1.dot(e1);
    00068         Vec3f e = -e1.dot(d);
    00069         Vec3f f = d.dot(d);
    00070
    00071         Vec3f detv = (a*c) - (b*b);
    00072         Vec3f sv   = (b*e) - (c*d);
    00073         Vec3f tv   = (b*d) - (a*e);
    00074
    00075         float det = detv.length();
    00076         float s   = sv.length();
    00077         float t   = tv.length();
    00078
    00079         if((s+t <= det) && (s>=0) && (t>=0)){
    00080             double inv = 1/det;
    00081             s *= inv;
    00082             t*= inv;
    00083             Vec3f result = v0 + e0*s + e1*t;
    00084             float d = result.distance(point);
    00085             if (d < dmin && d <= maxRadius){
    00086                 dmin = d;
    00087                 min_face = result;
    00088                 faceID = i;
    00089             }
    00090         }
    00091     }
    00092     cout << "Closest Point on Face to Query Point ";
    00093     cout<<min_face.x << " "<< min_face.y << " "<< min_face.z;
    00094     cout<<", Distance " << dmin <<endl ;
    00095     return std::pair<float,Vec3f>(dmin,min_face);
    00096 }
    00097
    00098 /*Helper function  to Ditance of point P from Edge (X0,X1)*/
    00099 float dist_lineseg_to_point(Vec3f x0, Vec3f x1, const Vec3f p, Vec3f &result){
    00100
    00101     Vec3f v = x1-x0;
    00102     Vec3f w = p-x0;
    00103
    00104     float c1 = w.dot(v);
    00105     float c2 = v.dot(v);
    00106
    00107     if (!(c1<0) && !(c2 <=c1)){
    00108         float b = c1/c2;
    00109         result = x0+ v*b;
    00110         return result.distance(p);
    00111     }
    00112     return INFINITY;
    00113 }
    00114
    00115 /* Loop thorough Adj List, find the distance from point to every edge*/
    00116 pair<float,Vec3f> ClosestPoint::minEdgeDistance(const Vec3f &point,float maxRadius){
    00117
    00118     Vec3f minEdgePoint;
    00119     Vec3f result;
    00120     int v1,v2;
    00121     float dmin = INFINITY;
    00122
    00123     vector<Vec3f> vertex_list = mesh.get_vertexList();
    00124     std::vector<set <int> > edgeList = mesh.get_edgeList();
    00125
    00126     for (int i =0; i<edgeList.size(); i++){
    00127
    00128         set <int> temp = edgeList[i];
    00129         Vec3f x1 = vertex_list[i];
    00130
    00131         for (set<int>::iterator j = temp.begin(); j != temp.end(); j++) {
    00132             int element = *j;
    00133             Vec3f x2 = vertex_list[element];
    00134             float d = dist_lineseg_to_point(x1,x2,point,result);
    00135             if (d < (maxRadius) && d<dmin){
    00136                 dmin = d;
    00137                 minEdgePoint = result;
    00138                 edge_in_sphere.insert(i);
    00139                 edge_in_sphere.insert(element);
    00140                 //cout << "After "<<point.x<<endl;
    00141                 v1 = i;
    00142                 v2 = element;
    00143             }
    00144         }
    00145     }
    00146
    00147     cout << "Closest Point on Edge to Query Point ";
    00148     cout << minEdgePoint.x <<" "<< minEdgePoint.y <<" "<< minEdgePoint.z;
    00149     cout << ", Distance " << dmin<<endl;
    00150     return pair<float,Vec3f> (dmin,minEdgePoint);
    00151 }
    00152
    00153 pair<float,Vec3f> ClosestPoint::minVertexDistance(const Vec3f &point, float maxRadius){
    00154
    00155     float dmin = INFINITY_FLOAT;
    00156     Vec3f closest;
    00157     float d;
    00158     vector<Vec3f> vertex_list = mesh.get_vertexList();
    00159
    00160     for(int i=0; i< vertex_list.size();i++){
    00161         d = vertex_list[i].distance(point);
    00162         if (d<dmin && d<= maxRadius){
    00163             dmin = d;
    00164             closest = vertex_list[i];
    00165             vlist_in_sphere.insert(i);
    00166         }
    00167     }
    00168     cout << "Closest Vertex to Query Point ";
    00169     cout << closest.x <<" "<< closest.y <<" "<< closest.z;
    00170     cout << ", Distance " << dmin<< endl;
    00171
    00172     return std::pair<float,Vec3f>(dmin,closest);
    00173 }
    00174
    00175 void ClosestPoint::makeEdgeList(){
    00176     int size = mesh.nverts;
    00177     set <int> temp;
    00178     std::vector<set <int> > edgeList;
    00179     vector<Face> face_list = mesh.get_faceList();
    00180
    00181     for(int i = 0; i< size; i++)
    00182         edgeList.push_back(temp);
    00183
    00184     //Loop through all faces, prepare adjacency list
    00185     for (int i =0 ; i< face_list.size(); i++){
    00186         int n = face_list[i].nverts;
    00187         std::vector<set <int> > :: iterator it;
    00188
    00189         /*Each face stores vertex ID list, convert it to 3 edges*/
    00190         for(int j = 0; j< n;  j++){
    00191             int idx1 = face_list[i].vid[j];
    00192             int idx2 = face_list[i].vid[(j+1) % n];
    00193
    00194             edgeList[idx1].insert(idx2);
    00195         }
    00196     }
    00197     mesh.set_edgeList(edgeList);
    00198 }
    00199
    00200 /*Find distance of point on face, of only those faces which have
    00201 vertices or edges within the maximum search radius from queryPoint*/
    00202
    00203 std::vector<Face> ClosestPoint::fetchFacesinSphere(){
    00204
    00205     std::set <int> valid_verts;
    00206
    00207     std::vector<Face> result;
    00208     vector<Face> face_list = mesh.get_faceList();
    00209
    00210     set_union(vlist_in_sphere.begin(),vlist_in_sphere.end(),
    00211                         edge_in_sphere.begin(), edge_in_sphere.end(),
    00212                         std::inserter(valid_verts,valid_verts.begin()));
    00213
    00214     for (int i =0; i< face_list.size(); i++){
    00215         /*Create set of current face vertices*/
    00216         std::set<int> temp;
    00217         for (int j =0; j<3; j++ ){
    00218             temp.insert(face_list[i].vid[j]);
    00219         }
    00220
    00221         /*Check interesction with Bounding Sphere*/
    00222         set<int> intersect;
    00223         set_intersection(temp.begin(),temp.end(),valid_verts.begin(),
    00224                         valid_verts.end(),
    00225                         std::inserter(intersect,intersect.begin()));
    00226         if (intersect.size()>0)
    00227             result.push_back(face_list[i]);
    00228     }
    00229     return result;
    00230 }

All Classes