Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	test/utility.cpp	Lines:	268	335	80.0 %
Date:	2024-02-09 12:57:42	Branches:	180	461	39.0 %


#include "utility.h"

#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>

#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>

#include <cstdio>
#include <cstddef>
#include <fstream>
#include <iostream>

namespace hpp {
namespace fcl {

BenchTimer::BenchTimer() {
#ifdef _WIN32
  QueryPerformanceFrequency(&frequency);
  startCount.QuadPart = 0;
  endCount.QuadPart = 0;
#else
  startCount.tv_sec = startCount.tv_usec = 0;
  endCount.tv_sec = endCount.tv_usec = 0;
#endif

  stopped = 0;
  startTimeInMicroSec = 0;
  endTimeInMicroSec = 0;
}

BenchTimer::~BenchTimer() {}

void BenchTimer::start() {
  stopped = 0;  // reset stop flag
#ifdef _WIN32
  QueryPerformanceCounter(&startCount);
#else
  gettimeofday(&startCount, NULL);
#endif
}

void BenchTimer::stop() {
  stopped = 1;  // set timer stopped flag

#ifdef _WIN32
  QueryPerformanceCounter(&endCount);
#else
  gettimeofday(&endCount, NULL);
#endif
}

double BenchTimer::getElapsedTimeInMicroSec() {
#ifdef _WIN32
  if (!stopped) QueryPerformanceCounter(&endCount);

  startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart);
  endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart);
#else
  if (!stopped) gettimeofday(&endCount, NULL);

  startTimeInMicroSec =
      ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec;
  endTimeInMicroSec =
      ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec;
#endif

  return endTimeInMicroSec - startTimeInMicroSec;
}

double BenchTimer::getElapsedTimeInMilliSec() {
  return this->getElapsedTimeInMicroSec() * 0.001;
}

double BenchTimer::getElapsedTimeInSec() {
  return this->getElapsedTimeInMicroSec() * 0.000001;
}

double BenchTimer::getElapsedTime() { return this->getElapsedTimeInMilliSec(); }

const Eigen::IOFormat vfmt = Eigen::IOFormat(
    Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
const Eigen::IOFormat pyfmt = Eigen::IOFormat(
    Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

const Vec3f UnitX = Vec3f(1, 0, 0);
const Vec3f UnitY = Vec3f(0, 1, 0);
const Vec3f UnitZ = Vec3f(0, 0, 1);

FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) {
  FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
  return (t * (rmax - rmin) + rmin);
}

void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
                 std::vector<Triangle>& triangles) {
  FILE* file = fopen(filename, "rb");
  if (!file) {
    std::cerr << "file not exist" << std::endl;
    return;
  }

  bool has_normal = false;
  bool has_texture = false;
  char line_buffer[2000];
  while (fgets(line_buffer, 2000, file)) {
    char* first_token = strtok(line_buffer, "\r\n\t ");
    if (!first_token || first_token[0] == '#' || first_token[0] == 0) continue;

    switch (first_token[0]) {
      case 'v': {
        if (first_token[1] == 'n') {
          strtok(NULL, "\t ");
          strtok(NULL, "\t ");
          strtok(NULL, "\t ");
          has_normal = true;
        } else if (first_token[1] == 't') {
          strtok(NULL, "\t ");
          strtok(NULL, "\t ");
          has_texture = true;
        } else {
          FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t "));
          FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t "));
          FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t "));
          Vec3f p(x, y, z);
          points.push_back(p);
        }
      } break;
      case 'f': {
        Triangle tri;
        char* data[30];
        int n = 0;
        while ((data[n] = strtok(NULL, "\t \r\n")) != NULL) {
          if (strlen(data[n])) n++;
        }

        for (int t = 0; t < (n - 2); ++t) {
          if ((!has_texture) && (!has_normal)) {
            tri[0] = (Triangle::index_type)(atoi(data[0]) - 1);
            tri[1] = (Triangle::index_type)(atoi(data[1]) - 1);
            tri[2] = (Triangle::index_type)(atoi(data[2]) - 1);
          } else {
            const char* v1;
            for (Triangle::index_type i = 0; i < 3; i++) {
              // vertex ID
              if (i == 0)
                v1 = data[0];
              else
                v1 = data[(Triangle::index_type)t + i];

              tri[i] = (Triangle::index_type)(atoi(v1) - 1);
            }
          }
          triangles.push_back(tri);
        }
      }
    }
  }
}

void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
                 std::vector<Triangle>& triangles) {
  std::ofstream os(filename);
  if (!os) {
    std::cerr << "file not exist" << std::endl;
    return;
  }

  for (std::size_t i = 0; i < points.size(); ++i) {
    os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2]
       << std::endl;
  }

  for (std::size_t i = 0; i < triangles.size(); ++i) {
    os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " "
       << triangles[i][2] + 1 << std::endl;
  }

  os.close();
}

#ifdef HPP_FCL_HAS_OCTOMAP
OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) {
  octomap::OcTreePtr_t octree(new octomap::OcTree(filename));
  if (octree->getResolution() != resolution) {
    std::ostringstream oss;
    oss << "Resolution of the OcTree is " << octree->getResolution()
        << " and not " << resolution;
    throw std::invalid_argument(oss.str());
  }
  return hpp::fcl::OcTree(octree);
}
#endif

void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) {
  FCL_REAL c1 = cos(a);
  FCL_REAL c2 = cos(b);
  FCL_REAL c3 = cos(c);
  FCL_REAL s1 = sin(a);
  FCL_REAL s2 = sin(b);
  FCL_REAL s3 = sin(c);

  R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3,
      -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3;
}

void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) {
  FCL_REAL x = rand_interval(extents[0], extents[3]);
  FCL_REAL y = rand_interval(extents[1], extents[4]);
  FCL_REAL z = rand_interval(extents[2], extents[5]);

  const FCL_REAL pi = 3.1415926;
  FCL_REAL a = rand_interval(0, 2 * pi);
  FCL_REAL b = rand_interval(0, 2 * pi);
  FCL_REAL c = rand_interval(0, 2 * pi);

  Matrix3f R;
  eulerToMatrix(a, b, c, R);
  Vec3f T(x, y, z);
  transform.setTransform(R, T);
}

void generateRandomTransforms(FCL_REAL extents[6],
                              std::vector<Transform3f>& transforms,
                              std::size_t n) {
  transforms.resize(n);
  for (std::size_t i = 0; i < n; ++i) {
    FCL_REAL x = rand_interval(extents[0], extents[3]);
    FCL_REAL y = rand_interval(extents[1], extents[4]);
    FCL_REAL z = rand_interval(extents[2], extents[5]);

    const FCL_REAL pi = 3.1415926;
    FCL_REAL a = rand_interval(0, 2 * pi);
    FCL_REAL b = rand_interval(0, 2 * pi);
    FCL_REAL c = rand_interval(0, 2 * pi);

    {
      Matrix3f R;
      eulerToMatrix(a, b, c, R);
      Vec3f T(x, y, z);
      transforms[i].setTransform(R, T);
    }
  }
}

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) {
  transforms.resize(n);
  transforms2.resize(n);
  for (std::size_t i = 0; i < n; ++i) {
    FCL_REAL x = rand_interval(extents[0], extents[3]);
    FCL_REAL y = rand_interval(extents[1], extents[4]);
    FCL_REAL z = rand_interval(extents[2], extents[5]);

    const FCL_REAL pi = 3.1415926;
    FCL_REAL a = rand_interval(0, 2 * pi);
    FCL_REAL b = rand_interval(0, 2 * pi);
    FCL_REAL c = rand_interval(0, 2 * pi);

    {
      Matrix3f R;
      eulerToMatrix(a, b, c, R);
      Vec3f T(x, y, z);
      transforms[i].setTransform(R, T);
    }

    FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
    FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
    FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);

    FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot);
    FCL_REAL deltab = rand_interval(-delta_rot, delta_rot);
    FCL_REAL deltac = rand_interval(-delta_rot, delta_rot);

    {
      Matrix3f R;
      eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
      Vec3f T(x + deltax, y + deltay, z + deltaz);
      transforms2[i].setTransform(R, T);
    }
  }
}

bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
                              void* cdata_) {
  CollisionData* cdata = static_cast<CollisionData*>(cdata_);
  const CollisionRequest& request = cdata->request;
  CollisionResult& result = cdata->result;

  if (cdata->done) return true;

  collide(o1, o2, request, result);

  if ((result.isCollision()) &&
      (result.numContacts() >= request.num_max_contacts))
    cdata->done = true;

  return cdata->done;
}

bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
                             void* cdata_, FCL_REAL& dist) {
  DistanceData* cdata = static_cast<DistanceData*>(cdata_);
  const DistanceRequest& request = cdata->request;
  DistanceResult& result = cdata->result;

  if (cdata->done) {
    dist = result.min_distance;
    return true;
  }

  distance(o1, o2, request, result);

  dist = result.min_distance;

  if (dist <= 0) return true;  // in collision or in touch

  return cdata->done;
}

std::string getNodeTypeName(NODE_TYPE node_type) {
  if (node_type == BV_UNKNOWN)
    return std::string("BV_UNKNOWN");
  else if (node_type == BV_AABB)
    return std::string("BV_AABB");
  else if (node_type == BV_OBB)
    return std::string("BV_OBB");
  else if (node_type == BV_RSS)
    return std::string("BV_RSS");
  else if (node_type == BV_kIOS)
    return std::string("BV_kIOS");
  else if (node_type == BV_OBBRSS)
    return std::string("BV_OBBRSS");
  else if (node_type == BV_KDOP16)
    return std::string("BV_KDOP16");
  else if (node_type == BV_KDOP18)
    return std::string("BV_KDOP18");
  else if (node_type == BV_KDOP24)
    return std::string("BV_KDOP24");
  else if (node_type == GEOM_BOX)
    return std::string("GEOM_BOX");
  else if (node_type == GEOM_SPHERE)
    return std::string("GEOM_SPHERE");
  else if (node_type == GEOM_CAPSULE)
    return std::string("GEOM_CAPSULE");
  else if (node_type == GEOM_CONE)
    return std::string("GEOM_CONE");
  else if (node_type == GEOM_CYLINDER)
    return std::string("GEOM_CYLINDER");
  else if (node_type == GEOM_CONVEX)
    return std::string("GEOM_CONVEX");
  else if (node_type == GEOM_PLANE)
    return std::string("GEOM_PLANE");
  else if (node_type == GEOM_HALFSPACE)
    return std::string("GEOM_HALFSPACE");
  else if (node_type == GEOM_TRIANGLE)
    return std::string("GEOM_TRIANGLE");
  else if (node_type == GEOM_OCTREE)
    return std::string("GEOM_OCTREE");
  else
    return std::string("invalid");
}

Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) {
  Quaternion3f q;
  q.w() = w;
  q.x() = x;
  q.y() = y;
  q.z() = z;
  return q;
}

std::ostream& operator<<(std::ostream& os, const Transform3f& tf) {
  return os << "[ " << tf.getTranslation().format(vfmt) << ", "
            << tf.getQuatRotation().coeffs().format(vfmt) << " ]";
}

std::size_t getNbRun(const int& argc, char const* const* argv,
                     std::size_t defaultValue) {
  for (int i = 0; i < argc; ++i)
    if (strcmp(argv[i], "--nb-run") == 0)
      if (i + 1 != argc) return (std::size_t)strtol(argv[i + 1], NULL, 10);
  return defaultValue;
}

void generateEnvironments(std::vector<CollisionObject*>& env,
                          FCL_REAL env_scale, std::size_t n) {
  FCL_REAL extents[] = {-env_scale, env_scale,  -env_scale,
                        env_scale,  -env_scale, env_scale};
  std::vector<Transform3f> transforms(n);

  generateRandomTransforms(extents, transforms, n);
  for (std::size_t i = 0; i < n; ++i) {
    Box* box = new Box(5, 10, 20);
    env.push_back(
        new CollisionObject(shared_ptr<CollisionGeometry>(box), transforms[i]));
    env.back()->collisionGeometry()->computeLocalAABB();
  }

  generateRandomTransforms(extents, transforms, n);
  for (std::size_t i = 0; i < n; ++i) {
    Sphere* sphere = new Sphere(30);
    env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(sphere),
                                      transforms[i]));
    env.back()->collisionGeometry()->computeLocalAABB();
  }

  generateRandomTransforms(extents, transforms, n);
  for (std::size_t i = 0; i < n; ++i) {
    Cylinder* cylinder = new Cylinder(10, 40);
    env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
                                      transforms[i]));
    env.back()->collisionGeometry()->computeLocalAABB();
  }
}

void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
                              FCL_REAL env_scale, std::size_t n) {
  FCL_REAL extents[] = {-env_scale, env_scale,  -env_scale,
                        env_scale,  -env_scale, env_scale};
  std::vector<Transform3f> transforms;

  generateRandomTransforms(extents, transforms, n);
  Box box(5, 10, 20);
  for (std::size_t i = 0; i < n; ++i) {
    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
    generateBVHModel(*model, box, Transform3f::Identity());
    env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
                                      transforms[i]));
    env.back()->collisionGeometry()->computeLocalAABB();
  }

  generateRandomTransforms(extents, transforms, n);
  Sphere sphere(30);
  for (std::size_t i = 0; i < n; ++i) {
    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
    generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16);
    env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
                                      transforms[i]));
    env.back()->collisionGeometry()->computeLocalAABB();
  }

  generateRandomTransforms(extents, transforms, n);
  Cylinder cylinder(10, 40);
  for (std::size_t i = 0; i < n; ++i) {
    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
    generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16);
    env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
                                      transforms[i]));
    env.back()->collisionGeometry()->computeLocalAABB();
  }
}

/// Takes a point and projects it onto the surface of the unit sphere
void toSphere(Vec3f& point) {
  assert(point.norm() > 1e-8);
  point /= point.norm();
}

/// Takes a point, projects it on the unit sphere and applies an ellipsoid
/// transformation to it. A point x belongs to the surface of an ellipsoid if
/// x^T * A * x = 1, where A = diag(1/r**2) with r being the radii of the
/// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y *
/// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) =
/// diag(r).
void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) {
  toSphere(point);
  point[0] *= ellipsoid.radii[0];
  point[1] *= ellipsoid.radii[1];
  point[2] *= ellipsoid.radii[2];
}

Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
  FCL_REAL PHI = (1 + std::sqrt(5)) / 2;

  // vertices
  Vec3f* pts = new Vec3f[12];
  pts[0] = Vec3f(-1, PHI, 0);
  pts[1] = Vec3f(1, PHI, 0);
  pts[2] = Vec3f(-1, -PHI, 0);
  pts[3] = Vec3f(1, -PHI, 0);

  pts[4] = Vec3f(0, -1, PHI);
  pts[5] = Vec3f(0, 1, PHI);
  pts[6] = Vec3f(0, -1, -PHI);
  pts[7] = Vec3f(0, 1, -PHI);

  pts[8] = Vec3f(PHI, 0, -1);
  pts[9] = Vec3f(PHI, 0, 1);
  pts[10] = Vec3f(-PHI, 0, -1);
  pts[11] = Vec3f(-PHI, 0, 1);

  for (int i = 0; i < 12; ++i) {
    toEllipsoid(pts[i], ellipsoid);
  }

  // faces
  Triangle* tris = new Triangle[20];
  tris[0].set(0, 11, 5);
  tris[1].set(0, 5, 1);
  tris[2].set(0, 1, 7);
  tris[3].set(0, 7, 10);
  tris[4].set(0, 10, 11);

  tris[5].set(1, 5, 9);
  tris[6].set(5, 11, 4);
  tris[7].set(11, 10, 2);
  tris[8].set(10, 7, 6);
  tris[9].set(7, 1, 8);

  tris[10].set(3, 9, 4);
  tris[11].set(3, 4, 2);
  tris[12].set(3, 2, 6);
  tris[13].set(3, 6, 8);
  tris[14].set(3, 8, 9);

  tris[15].set(4, 9, 5);
  tris[16].set(2, 4, 11);
  tris[17].set(6, 2, 10);
  tris[18].set(8, 6, 7);
  tris[19].set(9, 8, 1);
  return Convex<Triangle>(true,
                          pts,   // points
                          12,    // num_points
                          tris,  // triangles
                          20     // number of triangles
  );
}

}  // namespace fcl

}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include "utility.h"
2
3			#include <hpp/fcl/BV/BV.h>
4			#include <hpp/fcl/BVH/BVH_model.h>
5			#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
6
7			#include <hpp/fcl/collision.h>
8			#include <hpp/fcl/distance.h>
9
10			#include <cstdio>
11			#include <cstddef>
12			#include <fstream>
13			#include <iostream>
14
15			namespace hpp {
16			namespace fcl {
17
18		500	BenchTimer::BenchTimer() {
19			#ifdef _WIN32
20			QueryPerformanceFrequency(&frequency);
21			startCount.QuadPart = 0;
22			endCount.QuadPart = 0;
23			#else
24		500	startCount.tv_sec = startCount.tv_usec = 0;
25		500	endCount.tv_sec = endCount.tv_usec = 0;
26			#endif
27
28		500	stopped = 0;
29		500	startTimeInMicroSec = 0;
30		500	endTimeInMicroSec = 0;
31		500	}
32
33		500	BenchTimer::~BenchTimer() {}
34
35		36581	void BenchTimer::start() {
36		36581	stopped = 0; // reset stop flag
37			#ifdef _WIN32
38			QueryPerformanceCounter(&startCount);
39			#else
40		36581	gettimeofday(&startCount, NULL);
41			#endif
42		36581	}
43
44		36581	void BenchTimer::stop() {
45		36581	stopped = 1; // set timer stopped flag
46
47			#ifdef _WIN32
48			QueryPerformanceCounter(&endCount);
49			#else
50		36581	gettimeofday(&endCount, NULL);
51			#endif
52		36581	}
53
54		36581	double BenchTimer::getElapsedTimeInMicroSec() {
55			#ifdef _WIN32
56			if (!stopped) QueryPerformanceCounter(&endCount);
57
58			startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart);
59			endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart);
60			#else
61	✗✓	36581	if (!stopped) gettimeofday(&endCount, NULL);
62
63		36581	startTimeInMicroSec =
64		36581	((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec;
65		36581	endTimeInMicroSec =
66		36581	((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec;
67			#endif
68
69		36581	return endTimeInMicroSec - startTimeInMicroSec;
70			}
71
72		36540	double BenchTimer::getElapsedTimeInMilliSec() {
73		36540	return this->getElapsedTimeInMicroSec() * 0.001;
74			}
75
76			double BenchTimer::getElapsedTimeInSec() {
77			return this->getElapsedTimeInMicroSec() * 0.000001;
78			}
79
80		36540	double BenchTimer::getElapsedTime() { return this->getElapsedTimeInMilliSec(); }
81
82			const Eigen::IOFormat vfmt = Eigen::IOFormat(
83			Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
84			const Eigen::IOFormat pyfmt = Eigen::IOFormat(
85			Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
86
87			const Vec3f UnitX = Vec3f(1, 0, 0);
88			const Vec3f UnitY = Vec3f(0, 1, 0);
89			const Vec3f UnitZ = Vec3f(0, 0, 1);
90
91		324054	FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) {
92		324054	FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
93		324054	return (t * (rmax - rmin) + rmin);
94			}
95
96		16	void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
97			std::vector<Triangle>& triangles) {
98	✓✗	16	FILE* file = fopen(filename, "rb");
99	✗✓	16	if (!file) {
100			std::cerr << "file not exist" << std::endl;
101			return;
102			}
103
104		16	bool has_normal = false;
105		16	bool has_texture = false;
106			char line_buffer[2000];
107	✓✗✓✓	84560	while (fgets(line_buffer, 2000, file)) {
108		84544	char* first_token = strtok(line_buffer, "\r\n\t ");
109	✓✗✓✗ ✗✓	84544	if (!first_token \|\| first_token[0] == '#' \|\| first_token[0] == 0) continue;
110
111	✓✓✓	84544	switch (first_token[0]) {
112		63396	case 'v': {
113	✗✓	63396	if (first_token[1] == 'n') {
114			strtok(NULL, "\t ");
115			strtok(NULL, "\t ");
116			strtok(NULL, "\t ");
117			has_normal = true;
118	✗✓	63396	} else if (first_token[1] == 't') {
119			strtok(NULL, "\t ");
120			strtok(NULL, "\t ");
121			has_texture = true;
122			} else {
123		63396	FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t "));
124		63396	FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t "));
125		63396	FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t "));
126	✓✗	63396	Vec3f p(x, y, z);
127	✓✗	63396	points.push_back(p);
128			}
129		63396	} break;
130		21132	case 'f': {
131		21132	Triangle tri;
132			char* data[30];
133		21132	int n = 0;
134	✓✓	84528	while ((data[n] = strtok(NULL, "\t \r\n")) != NULL) {
135	✓✗	63396	if (strlen(data[n])) n++;
136			}
137
138	✓✓	42264	for (int t = 0; t < (n - 2); ++t) {
139	✓✗✓✗	21132	if ((!has_texture) && (!has_normal)) {
140		21132	tri[0] = (Triangle::index_type)(atoi(data[0]) - 1);
141		21132	tri[1] = (Triangle::index_type)(atoi(data[1]) - 1);
142		21132	tri[2] = (Triangle::index_type)(atoi(data[2]) - 1);
143			} else {
144			const char* v1;
145			for (Triangle::index_type i = 0; i < 3; i++) {
146			// vertex ID
147			if (i == 0)
148			v1 = data[0];
149			else
150			v1 = data[(Triangle::index_type)t + i];
151
152			tri[i] = (Triangle::index_type)(atoi(v1) - 1);
153			}
154			}
155	✓✗	21132	triangles.push_back(tri);
156			}
157			}
158			}
159			}
160			}
161
162			void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
163			std::vector<Triangle>& triangles) {
164			std::ofstream os(filename);
165			if (!os) {
166			std::cerr << "file not exist" << std::endl;
167			return;
168			}
169
170			for (std::size_t i = 0; i < points.size(); ++i) {
171			os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2]
172			<< std::endl;
173			}
174
175			for (std::size_t i = 0; i < triangles.size(); ++i) {
176			os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " "
177			<< triangles[i][2] + 1 << std::endl;
178			}
179
180			os.close();
181			}
182
183			#ifdef HPP_FCL_HAS_OCTOMAP
184		1	OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) {
185	✓✗✓✗ ✓✗✓✗	1	octomap::OcTreePtr_t octree(new octomap::OcTree(filename));
186	✓✗✗✓	1	if (octree->getResolution() != resolution) {
187			std::ostringstream oss;
188			oss << "Resolution of the OcTree is " << octree->getResolution()
189			<< " and not " << resolution;
190			throw std::invalid_argument(oss.str());
191			}
192	✓✗	2	return hpp::fcl::OcTree(octree);
193			}
194			#endif
195
196		54009	void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) {
197		54009	FCL_REAL c1 = cos(a);
198		54009	FCL_REAL c2 = cos(b);
199		54009	FCL_REAL c3 = cos(c);
200		54009	FCL_REAL s1 = sin(a);
201		54009	FCL_REAL s2 = sin(b);
202		54009	FCL_REAL s3 = sin(c);
203
204	✓✗✓✗ ✓✗✓✗ ✓✗	54009	R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3,
205	✓✗✓✗ ✓✗✓✗	54009	-c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3;
206		54009	}
207
208		1137	void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) {
209		1137	FCL_REAL x = rand_interval(extents[0], extents[3]);
210		1137	FCL_REAL y = rand_interval(extents[1], extents[4]);
211		1137	FCL_REAL z = rand_interval(extents[2], extents[5]);
212
213		1137	const FCL_REAL pi = 3.1415926;
214		1137	FCL_REAL a = rand_interval(0, 2 * pi);
215		1137	FCL_REAL b = rand_interval(0, 2 * pi);
216		1137	FCL_REAL c = rand_interval(0, 2 * pi);
217
218	✓✗	1137	Matrix3f R;
219	✓✗	1137	eulerToMatrix(a, b, c, R);
220	✓✗	1137	Vec3f T(x, y, z);
221	✓✗	1137	transform.setTransform(R, T);
222		1137	}
223
224		386	void generateRandomTransforms(FCL_REAL extents[6],
225			std::vector<Transform3f>& transforms,
226			std::size_t n) {
227		386	transforms.resize(n);
228	✓✓	53254	for (std::size_t i = 0; i < n; ++i) {
229		52868	FCL_REAL x = rand_interval(extents[0], extents[3]);
230		52868	FCL_REAL y = rand_interval(extents[1], extents[4]);
231		52868	FCL_REAL z = rand_interval(extents[2], extents[5]);
232
233		52868	const FCL_REAL pi = 3.1415926;
234		52868	FCL_REAL a = rand_interval(0, 2 * pi);
235		52868	FCL_REAL b = rand_interval(0, 2 * pi);
236		52868	FCL_REAL c = rand_interval(0, 2 * pi);
237
238			{
239	✓✗	52868	Matrix3f R;
240	✓✗	52868	eulerToMatrix(a, b, c, R);
241	✓✗	52868	Vec3f T(x, y, z);
242	✓✗	52868	transforms[i].setTransform(R, T);
243			}
244			}
245		386	}
246
247		1	void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3],
248			FCL_REAL delta_rot,
249			std::vector<Transform3f>& transforms,
250			std::vector<Transform3f>& transforms2,
251			std::size_t n) {
252		1	transforms.resize(n);
253		1	transforms2.resize(n);
254	✓✓	3	for (std::size_t i = 0; i < n; ++i) {
255		2	FCL_REAL x = rand_interval(extents[0], extents[3]);
256		2	FCL_REAL y = rand_interval(extents[1], extents[4]);
257		2	FCL_REAL z = rand_interval(extents[2], extents[5]);
258
259		2	const FCL_REAL pi = 3.1415926;
260		2	FCL_REAL a = rand_interval(0, 2 * pi);
261		2	FCL_REAL b = rand_interval(0, 2 * pi);
262		2	FCL_REAL c = rand_interval(0, 2 * pi);
263
264			{
265	✓✗	2	Matrix3f R;
266	✓✗	2	eulerToMatrix(a, b, c, R);
267	✓✗	2	Vec3f T(x, y, z);
268	✓✗	2	transforms[i].setTransform(R, T);
269			}
270
271		2	FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
272		2	FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
273		2	FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
274
275		2	FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot);
276		2	FCL_REAL deltab = rand_interval(-delta_rot, delta_rot);
277		2	FCL_REAL deltac = rand_interval(-delta_rot, delta_rot);
278
279			{
280	✓✗	2	Matrix3f R;
281	✓✗	2	eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
282	✓✗	2	Vec3f T(x + deltax, y + deltay, z + deltaz);
283	✓✗	2	transforms2[i].setTransform(R, T);
284			}
285			}
286		1	}
287
288		1224140	bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
289			void* cdata_) {
290		1224140	CollisionData* cdata = static_cast<CollisionData*>(cdata_);
291		1224140	const CollisionRequest& request = cdata->request;
292		1224140	CollisionResult& result = cdata->result;
293
294	✗✓	1224140	if (cdata->done) return true;
295
296		1224140	collide(o1, o2, request, result);
297
298	✓✓✓✓	1225197	if ((result.isCollision()) &&
299	✓✓	1057	(result.numContacts() >= request.num_max_contacts))
300		753	cdata->done = true;
301
302		1224140	return cdata->done;
303			}
304
305		17230	bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
306			void* cdata_, FCL_REAL& dist) {
307		17230	DistanceData* cdata = static_cast<DistanceData*>(cdata_);
308		17230	const DistanceRequest& request = cdata->request;
309		17230	DistanceResult& result = cdata->result;
310
311	✗✓	17230	if (cdata->done) {
312			dist = result.min_distance;
313			return true;
314			}
315
316		17230	distance(o1, o2, request, result);
317
318		17230	dist = result.min_distance;
319
320	✗✓	17230	if (dist <= 0) return true; // in collision or in touch
321
322		17230	return cdata->done;
323			}
324
325		4	std::string getNodeTypeName(NODE_TYPE node_type) {
326	✗✓	4	if (node_type == BV_UNKNOWN)
327			return std::string("BV_UNKNOWN");
328	✗✓	4	else if (node_type == BV_AABB)
329			return std::string("BV_AABB");
330	✓✓	4	else if (node_type == BV_OBB)
331	✓✗	1	return std::string("BV_OBB");
332	✓✓	3	else if (node_type == BV_RSS)
333	✓✗	1	return std::string("BV_RSS");
334	✓✓	2	else if (node_type == BV_kIOS)
335	✓✗	1	return std::string("BV_kIOS");
336	✓✗	1	else if (node_type == BV_OBBRSS)
337	✓✗	1	return std::string("BV_OBBRSS");
338			else if (node_type == BV_KDOP16)
339			return std::string("BV_KDOP16");
340			else if (node_type == BV_KDOP18)
341			return std::string("BV_KDOP18");
342			else if (node_type == BV_KDOP24)
343			return std::string("BV_KDOP24");
344			else if (node_type == GEOM_BOX)
345			return std::string("GEOM_BOX");
346			else if (node_type == GEOM_SPHERE)
347			return std::string("GEOM_SPHERE");
348			else if (node_type == GEOM_CAPSULE)
349			return std::string("GEOM_CAPSULE");
350			else if (node_type == GEOM_CONE)
351			return std::string("GEOM_CONE");
352			else if (node_type == GEOM_CYLINDER)
353			return std::string("GEOM_CYLINDER");
354			else if (node_type == GEOM_CONVEX)
355			return std::string("GEOM_CONVEX");
356			else if (node_type == GEOM_PLANE)
357			return std::string("GEOM_PLANE");
358			else if (node_type == GEOM_HALFSPACE)
359			return std::string("GEOM_HALFSPACE");
360			else if (node_type == GEOM_TRIANGLE)
361			return std::string("GEOM_TRIANGLE");
362			else if (node_type == GEOM_OCTREE)
363			return std::string("GEOM_OCTREE");
364			else
365			return std::string("invalid");
366			}
367
368		7	Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) {
369		7	Quaternion3f q;
370		7	q.w() = w;
371		7	q.x() = x;
372		7	q.y() = y;
373		7	q.z() = z;
374		7	return q;
375			}
376
377			std::ostream& operator<<(std::ostream& os, const Transform3f& tf) {
378			return os << "[ " << tf.getTranslation().format(vfmt) << ", "
379			<< tf.getQuatRotation().coeffs().format(vfmt) << " ]";
380			}
381
382		5	std::size_t getNbRun(const int& argc, char const* const* argv,
383			std::size_t defaultValue) {
384	✓✓	10	for (int i = 0; i < argc; ++i)
385	✗✓	5	if (strcmp(argv[i], "--nb-run") == 0)
386			if (i + 1 != argc) return (std::size_t)strtol(argv[i + 1], NULL, 10);
387		5	return defaultValue;
388			}
389
390		69	void generateEnvironments(std::vector<CollisionObject*>& env,
391			FCL_REAL env_scale, std::size_t n) {
392		69	FCL_REAL extents[] = {-env_scale, env_scale, -env_scale,
393		69	env_scale, -env_scale, env_scale};
394	✓✗	138	std::vector<Transform3f> transforms(n);
395
396	✓✗	69	generateRandomTransforms(extents, transforms, n);
397	✓✓	4523	for (std::size_t i = 0; i < n; ++i) {
398	✓✗✓✗	4454	Box* box = new Box(5, 10, 20);
399		4454	env.push_back(
400	✓✗✓✗ ✓✗✓✗	4454	new CollisionObject(shared_ptr<CollisionGeometry>(box), transforms[i]));
401	✓✗	4454	env.back()->collisionGeometry()->computeLocalAABB();
402			}
403
404	✓✗	69	generateRandomTransforms(extents, transforms, n);
405	✓✓	4523	for (std::size_t i = 0; i < n; ++i) {
406	✓✗✓✗	4454	Sphere* sphere = new Sphere(30);
407	✓✗✓✗	8908	env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(sphere),
408	✓✗✓✗	4454	transforms[i]));
409	✓✗	4454	env.back()->collisionGeometry()->computeLocalAABB();
410			}
411
412	✓✗	69	generateRandomTransforms(extents, transforms, n);
413	✓✓	4523	for (std::size_t i = 0; i < n; ++i) {
414	✓✗✓✗	4454	Cylinder* cylinder = new Cylinder(10, 40);
415	✓✗✓✗	8908	env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
416	✓✗✓✗	4454	transforms[i]));
417	✓✗	4454	env.back()->collisionGeometry()->computeLocalAABB();
418			}
419		69	}
420
421		44	void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
422			FCL_REAL env_scale, std::size_t n) {
423		44	FCL_REAL extents[] = {-env_scale, env_scale, -env_scale,
424		44	env_scale, -env_scale, env_scale};
425		88	std::vector<Transform3f> transforms;
426
427	✓✗	44	generateRandomTransforms(extents, transforms, n);
428	✓✗	88	Box box(5, 10, 20);
429	✓✓	411	for (std::size_t i = 0; i < n; ++i) {
430	✓✗✓✗	367	BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
431	✓✗✓✗	367	generateBVHModel(*model, box, Transform3f::Identity());
432	✓✗✓✗	734	env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
433	✓✗✓✗	367	transforms[i]));
434	✓✗	367	env.back()->collisionGeometry()->computeLocalAABB();
435			}
436
437	✓✗	44	generateRandomTransforms(extents, transforms, n);
438	✓✗	88	Sphere sphere(30);
439	✓✓	411	for (std::size_t i = 0; i < n; ++i) {
440	✓✗✓✗	367	BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
441	✓✗✓✗	367	generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16);
442	✓✗✓✗	734	env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
443	✓✗✓✗	367	transforms[i]));
444	✓✗	367	env.back()->collisionGeometry()->computeLocalAABB();
445			}
446
447	✓✗	44	generateRandomTransforms(extents, transforms, n);
448	✓✗	88	Cylinder cylinder(10, 40);
449	✓✓	411	for (std::size_t i = 0; i < n; ++i) {
450	✓✗✓✗	367	BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
451	✓✗✓✗	367	generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16);
452	✓✗✓✗	734	env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
453	✓✗✓✗	367	transforms[i]));
454	✓✗	367	env.back()->collisionGeometry()->computeLocalAABB();
455			}
456		44	}
457
458			/// Takes a point and projects it onto the surface of the unit sphere
459		96	void toSphere(Vec3f& point) {
460	✗✓	96	assert(point.norm() > 1e-8);
461	✓✗	96	point /= point.norm();
462		96	}
463
464			/// Takes a point, projects it on the unit sphere and applies an ellipsoid
465			/// transformation to it. A point x belongs to the surface of an ellipsoid if
466			/// x^T * A * x = 1, where A = diag(1/r**2) with r being the radii of the
467			/// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y *
468			/// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) =
469			/// diag(r).
470		96	void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) {
471		96	toSphere(point);
472		96	point[0] *= ellipsoid.radii[0];
473		96	point[1] *= ellipsoid.radii[1];
474		96	point[2] *= ellipsoid.radii[2];
475		96	}
476
477		8	Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
478		8	FCL_REAL PHI = (1 + std::sqrt(5)) / 2;
479
480			// vertices
481	✓✗✓✓ ✓✗	104	Vec3f* pts = new Vec3f[12];
482	✓✗	8	pts[0] = Vec3f(-1, PHI, 0);
483	✓✗	8	pts[1] = Vec3f(1, PHI, 0);
484	✓✗	8	pts[2] = Vec3f(-1, -PHI, 0);
485	✓✗	8	pts[3] = Vec3f(1, -PHI, 0);
486
487	✓✗	8	pts[4] = Vec3f(0, -1, PHI);
488	✓✗	8	pts[5] = Vec3f(0, 1, PHI);
489	✓✗	8	pts[6] = Vec3f(0, -1, -PHI);
490	✓✗	8	pts[7] = Vec3f(0, 1, -PHI);
491
492	✓✗	8	pts[8] = Vec3f(PHI, 0, -1);
493	✓✗	8	pts[9] = Vec3f(PHI, 0, 1);
494	✓✗	8	pts[10] = Vec3f(-PHI, 0, -1);
495	✓✗	8	pts[11] = Vec3f(-PHI, 0, 1);
496
497	✓✓	104	for (int i = 0; i < 12; ++i) {
498	✓✗	96	toEllipsoid(pts[i], ellipsoid);
499			}
500
501			// faces
502	✓✗✓✓	168	Triangle* tris = new Triangle[20];
503		8	tris[0].set(0, 11, 5);
504		8	tris[1].set(0, 5, 1);
505		8	tris[2].set(0, 1, 7);
506		8	tris[3].set(0, 7, 10);
507		8	tris[4].set(0, 10, 11);
508
509		8	tris[5].set(1, 5, 9);
510		8	tris[6].set(5, 11, 4);
511		8	tris[7].set(11, 10, 2);
512		8	tris[8].set(10, 7, 6);
513		8	tris[9].set(7, 1, 8);
514
515		8	tris[10].set(3, 9, 4);
516		8	tris[11].set(3, 4, 2);
517		8	tris[12].set(3, 2, 6);
518		8	tris[13].set(3, 6, 8);
519		8	tris[14].set(3, 8, 9);
520
521		8	tris[15].set(4, 9, 5);
522		8	tris[16].set(2, 4, 11);
523		8	tris[17].set(6, 2, 10);
524		8	tris[18].set(8, 6, 7);
525		8	tris[19].set(9, 8, 1);
526			return Convex<Triangle>(true,
527			pts, // points
528			12, // num_points
529			tris, // triangles
530			20 // number of triangles
531	✓✗	16	);
532			}
533
534			} // namespace fcl
535
536			} // namespace hpp