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;
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;
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
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;
00356
00357 return cdata->done;
00358 }
00359
00360
00361 }