test_fcl_utility.cpp
Go to the documentation of this file.
00001 #include "test_fcl_utility.h"
00002 #include "fcl/collision.h"
00003 #include "fcl/distance.h"
00004 #include <cstdio>
00005 #include <cstddef>
00006 
00007 namespace fcl
00008 {
00009 
00010 
00011 Timer::Timer()
00012 {
00013 #ifdef _WIN32
00014   QueryPerformanceFrequency(&frequency);
00015   startCount.QuadPart = 0;
00016   endCount.QuadPart = 0;
00017 #else
00018   startCount.tv_sec = startCount.tv_usec = 0;
00019   endCount.tv_sec = endCount.tv_usec = 0;
00020 #endif
00021 
00022   stopped = 0;
00023   startTimeInMicroSec = 0;
00024   endTimeInMicroSec = 0;
00025 }
00026 
00027 
00028 Timer::~Timer()
00029 {
00030 }
00031 
00032 
00033 void Timer::start()
00034 {
00035   stopped = 0; // reset stop flag
00036 #ifdef _WIN32
00037   QueryPerformanceCounter(&startCount);
00038 #else
00039   gettimeofday(&startCount, NULL);
00040 #endif
00041 }
00042 
00043 
00044 void Timer::stop()
00045 {
00046   stopped = 1; // set timer stopped flag
00047 
00048 #ifdef _WIN32
00049   QueryPerformanceCounter(&endCount);
00050 #else
00051   gettimeofday(&endCount, NULL);
00052 #endif
00053 }
00054 
00055 
00056 double Timer::getElapsedTimeInMicroSec()
00057 {
00058 #ifdef _WIN32
00059   if(!stopped)
00060     QueryPerformanceCounter(&endCount);
00061 
00062   startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart);
00063   endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart);
00064 #else
00065   if(!stopped)
00066     gettimeofday(&endCount, NULL);
00067 
00068   startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec;
00069   endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec;
00070 #endif
00071 
00072   return endTimeInMicroSec - startTimeInMicroSec;
00073 }
00074 
00075 
00076 double Timer::getElapsedTimeInMilliSec()
00077 {
00078   return this->getElapsedTimeInMicroSec() * 0.001;
00079 }
00080 
00081 
00082 double Timer::getElapsedTimeInSec()
00083 {
00084   return this->getElapsedTimeInMicroSec() * 0.000001;
00085 }
00086 
00087 
00088 double Timer::getElapsedTime()
00089 {
00090   return this->getElapsedTimeInMilliSec();
00091 }
00092 
00093 
00094 FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax)
00095 {
00096   FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
00097   return (t * (rmax - rmin) + rmin);
00098 }
00099 
00100 void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles)
00101 {
00102 
00103   FILE* file = fopen(filename, "rb");
00104   if(!file)
00105   {
00106     std::cerr << "file not exist" << std::endl;
00107     return;
00108   }
00109 
00110   bool has_normal = false;
00111   bool has_texture = false;
00112   char line_buffer[2000];
00113   while(fgets(line_buffer, 2000, file))
00114   {
00115     char* first_token = strtok(line_buffer, "\r\n\t ");
00116     if(!first_token || first_token[0] == '#' || first_token[0] == 0)
00117       continue;
00118 
00119     switch(first_token[0])
00120     {
00121     case 'v':
00122       {
00123         if(first_token[1] == 'n')
00124         {
00125           strtok(NULL, "\t ");
00126           strtok(NULL, "\t ");
00127           strtok(NULL, "\t ");
00128           has_normal = true;
00129         }
00130         else if(first_token[1] == 't')
00131         {
00132           strtok(NULL, "\t ");
00133           strtok(NULL, "\t ");
00134           has_texture = true;
00135         }
00136         else
00137         {
00138           FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t "));
00139           FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t "));
00140           FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t "));
00141           Vec3f p(x, y, z);
00142           points.push_back(p);
00143         }
00144       }
00145       break;
00146     case 'f':
00147       {
00148         Triangle tri;
00149         char* data[30];
00150         int n = 0;
00151         while((data[n] = strtok(NULL, "\t \r\n")) != NULL)
00152         {
00153           if(strlen(data[n]))
00154             n++;
00155         }
00156 
00157         for(int t = 0; t < (n - 2); ++t)
00158         {
00159           if((!has_texture) && (!has_normal))
00160           {
00161             tri[0] = atoi(data[0]) - 1;
00162             tri[1] = atoi(data[1]) - 1;
00163             tri[2] = atoi(data[2]) - 1;
00164           }
00165           else
00166           {
00167             const char *v1;
00168             for(int i = 0; i < 3; i++)
00169             {
00170               // vertex ID
00171               if(i == 0)
00172                 v1 = data[0];
00173               else
00174                 v1 = data[t + i];
00175 
00176               tri[i] = atoi(v1) - 1;
00177             }
00178           }
00179           triangles.push_back(tri);
00180         }
00181       }
00182     }
00183   }
00184 }
00185 
00186 
00187 void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R)
00188 {
00189   FCL_REAL c1 = cos(a);
00190   FCL_REAL c2 = cos(b);
00191   FCL_REAL c3 = cos(c);
00192   FCL_REAL s1 = sin(a);
00193   FCL_REAL s2 = sin(b);
00194   FCL_REAL s3 = sin(c);
00195 
00196   R.setValue(c1 * c2, - c2 * s1, s2,
00197              c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3,
00198              s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
00199 }
00200 
00201 void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform)
00202 {
00203   FCL_REAL x = rand_interval(extents[0], extents[3]);
00204   FCL_REAL y = rand_interval(extents[1], extents[4]);
00205   FCL_REAL z = rand_interval(extents[2], extents[5]);
00206 
00207   const FCL_REAL pi = 3.1415926;
00208   FCL_REAL a = rand_interval(0, 2 * pi);
00209   FCL_REAL b = rand_interval(0, 2 * pi);
00210   FCL_REAL c = rand_interval(0, 2 * pi);
00211 
00212   Matrix3f R;
00213   eulerToMatrix(a, b, c, R);
00214   Vec3f T(x, y, z);
00215   transform.setTransform(R, T);
00216 }
00217 
00218 
00219 void generateRandomTransforms(FCL_REAL extents[6], std::vector<Transform3f>& transforms, std::size_t n)
00220 {
00221   transforms.resize(n);
00222   for(std::size_t i = 0; i < n; ++i)
00223   {
00224     FCL_REAL x = rand_interval(extents[0], extents[3]);
00225     FCL_REAL y = rand_interval(extents[1], extents[4]);
00226     FCL_REAL z = rand_interval(extents[2], extents[5]);
00227 
00228     const FCL_REAL pi = 3.1415926;
00229     FCL_REAL a = rand_interval(0, 2 * pi);
00230     FCL_REAL b = rand_interval(0, 2 * pi);
00231     FCL_REAL c = rand_interval(0, 2 * pi);
00232 
00233     {
00234       Matrix3f R;
00235       eulerToMatrix(a, b, c, R);
00236       Vec3f T(x, y, z);
00237       transforms[i].setTransform(R, T);
00238     }
00239   }
00240 }
00241 
00242 
00243 void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, std::size_t n)
00244 {
00245   transforms.resize(n);
00246   transforms2.resize(n);
00247   for(std::size_t i = 0; i < n; ++i)
00248   {
00249     FCL_REAL x = rand_interval(extents[0], extents[3]);
00250     FCL_REAL y = rand_interval(extents[1], extents[4]);
00251     FCL_REAL z = rand_interval(extents[2], extents[5]);
00252 
00253     const FCL_REAL pi = 3.1415926;
00254     FCL_REAL a = rand_interval(0, 2 * pi);
00255     FCL_REAL b = rand_interval(0, 2 * pi);
00256     FCL_REAL c = rand_interval(0, 2 * pi);
00257 
00258     {
00259       Matrix3f R;
00260       eulerToMatrix(a, b, c, R);
00261       Vec3f T(x, y, z);
00262       transforms[i].setTransform(R, T);
00263     }
00264 
00265     FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
00266     FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
00267     FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
00268 
00269     FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot);
00270     FCL_REAL deltab = rand_interval(-delta_rot, delta_rot);
00271     FCL_REAL deltac = rand_interval(-delta_rot, delta_rot);
00272 
00273     {
00274       Matrix3f R;
00275       eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
00276       Vec3f T(x + deltax, y + deltay, z + deltaz);
00277       transforms2[i].setTransform(R, T);
00278     }
00279   }
00280 }
00281 
00282 void generateRandomTransform_ccd(FCL_REAL extents[6], std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n,
00283                                  const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
00284                                  const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2)
00285 {
00286   transforms.resize(n);
00287   transforms2.resize(n);
00288 
00289   for(std::size_t i = 0; i < n;)
00290   {
00291     FCL_REAL x = rand_interval(extents[0], extents[3]);
00292     FCL_REAL y = rand_interval(extents[1], extents[4]);
00293     FCL_REAL z = rand_interval(extents[2], extents[5]);
00294 
00295     const FCL_REAL pi = 3.1415926;
00296     FCL_REAL a = rand_interval(0, 2 * pi);
00297     FCL_REAL b = rand_interval(0, 2 * pi);
00298     FCL_REAL c = rand_interval(0, 2 * pi);
00299 
00300 
00301     Matrix3f R;
00302     eulerToMatrix(a, b, c, R);
00303     Vec3f T(x, y, z);    
00304     Transform3f tf(R, T);
00305 
00306     std::vector<std::pair<int, int> > results;
00307     {
00308       transforms[i] = tf;
00309 
00310       FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
00311       FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
00312       FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
00313 
00314       FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot);
00315       FCL_REAL deltab = rand_interval(-delta_rot, delta_rot);
00316       FCL_REAL deltac = rand_interval(-delta_rot, delta_rot);
00317 
00318       Matrix3f R2;
00319       eulerToMatrix(a + deltaa, b + deltab, c + deltac, R2);
00320       Vec3f T2(x + deltax, y + deltay, z + deltaz);
00321       transforms2[i].setTransform(R2, T2);
00322       ++i;
00323     }
00324   }
00325 }
00326 
00327 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
00328 {
00329   CollisionData* cdata = static_cast<CollisionData*>(cdata_);
00330   const CollisionRequest& request = cdata->request;
00331   CollisionResult& result = cdata->result;
00332 
00333   if(cdata->done) return true;
00334 
00335   collide(o1, o2, request, result);
00336 
00337   if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts))
00338     cdata->done = true;
00339 
00340   return cdata->done;
00341 }
00342 
00343 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist)
00344 {
00345   DistanceData* cdata = static_cast<DistanceData*>(cdata_);
00346   const DistanceRequest& request = cdata->request;
00347   DistanceResult& result = cdata->result;
00348 
00349   if(cdata->done) { dist = result.min_distance; return true; }
00350 
00351   distance(o1, o2, request, result);
00352   
00353   dist = result.min_distance;
00354 
00355   if(dist <= 0) return true; // in collision or in touch
00356 
00357   return cdata->done;
00358 }
00359 
00360 
00361 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:31