MeshKit  1.0
test_point_edge_dist.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <math.h>
00003 
00004 double dot_product( double vect0[], double vect1[] ) {                                        
00005   return vect0[0]*vect1[0] + vect0[1]*vect1[1] + vect0[2]*vect1[2];                           
00006 }                                                                                             
00007                                                                                               
00008 void cross_product( double vect0[], double vect1[], double vect2[] ) {                        
00009   vect2[0] = vect0[1]*vect1[2] - vect0[2]*vect1[1];                                           
00010   vect2[1] = vect0[2]*vect1[0] - vect0[0]*vect1[2];                                           
00011   vect2[2] = vect0[0]*vect1[1] - vect0[1]*vect1[0];                                           
00012 }
00013 
00014 double dist_between_verts( double coords0[], double coords1[] ) {                             
00015   return sqrt( (coords0[0]-coords1[0])*(coords0[0]-coords1[0]) +                              
00016                (coords0[1]-coords1[1])*(coords0[1]-coords1[1]) +                              
00017                (coords0[2]-coords1[2])*(coords0[2]-coords1[2]) );                             
00018 }
00019 
00020 // from http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=geometry1                    
00021 double point_edge_dist( double a[], double b[], double c[] ) {                                
00022   double ab[3], bc[3], ba[3], ac[3];                                                          
00023   for(int i=0; i<3; i++) {                                                                    
00024     ab[i] = b[i] - a[i];                                                                      
00025     bc[i] = c[i] - b[i];                                                                      
00026     ba[i] = a[i] - b[i];                                                                      
00027     ac[i] = c[i] - a[i];                                                                      
00028   }                                                                                           
00029                                                                                               
00030   // find the magnitude of the cross product and test the line                                
00031   double cross_prod[3];                                                                       
00032   cross_product(ab, ac, cross_prod);                                                          
00033   double dist = sqrt(dot_product(cross_prod,cross_prod)) / dist_between_verts(a,b);           
00034                                                                                               
00035   // test endpoint1                                                                           
00036   double dot1 = dot_product( ab, bc );                                                        
00037   if (dot1 > 0) {                                                                             
00038     std::cout << "point_edge_dist=" << dist_between_verts(b,c) << " at endpt1" << std::endl;  
00039     return dist_between_verts(b,c);                                                           
00040   }                                                                                           
00041                                                                                               
00042   // test endpoint0                                                                           
00043   double dot2 = dot_product( ba, ac );                                                        
00044   if (dot2 > 0) {                                                                             
00045     std::cout << "point_edge_dist=" << dist_between_verts(a,c) << " at endpt0" << std::endl;  
00046     return dist_between_verts(a,c);                                                           
00047   }                                                                                           
00048                                                                                               
00049   std::cout << "point_edge_dist=" << fabs(dist) << " at middle" << std::endl;                 
00050   return fabs(dist);                                                                          
00051 }     
00052 
00053 int main() {
00054   double a[] = { 859.5510374 ,90.1085829 ,1.929114349 };
00055   double b[] = { 859.5571858, 90.1057543, 2.56411115 };
00056   double c[] = { 859.556885,    90.105728,     2.564118 };
00057   std::cout << point_edge_dist( a,b,c) << std::endl;
00058 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines