utility.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef TEST_HPP_FCL_UTILITY_H
#define TEST_HPP_FCL_UTILITY_H

#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/broadphase/default_broadphase_callbacks.h>
#include <hpp/fcl/shape/convex.h>

#ifdef HPP_FCL_HAS_OCTOMAP
#include <hpp/fcl/octree.h>
#endif

#ifdef _WIN32
#define NOMINMAX  // required to avoid compilation errors with Visual Studio
                  // 2010
#include <windows.h>
#else
#include <sys/time.h>
#endif

#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)         \
  BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision),    \
                      "check " #Va ".isApprox(" #Vb       \
                      ") failed "                         \
                      "[\n"                               \
                          << (Va).transpose() << "\n!=\n" \
                          << (Vb).transpose() << "\n]")
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision)                              \
  BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), "check " #Va            \
                                                       ".isApprox(" #Vb        \
                                                       ") failed "             \
                                                       "[\n"                   \
                                                           << (Va) << "\n!=\n" \
                                                           << (Vb) << "\n]")

namespace octomap {
#ifdef HPP_FCL_HAS_OCTOMAP
typedef hpp::fcl::shared_ptr<octomap::OcTree> OcTreePtr_t;
#endif
}  // namespace octomap
namespace hpp {
namespace fcl {

class BenchTimer {
 public:
  BenchTimer();
  ~BenchTimer();

  void start();                       
  void stop();                        
  double getElapsedTime();            
  double getElapsedTimeInSec();       
  double getElapsedTimeInMilliSec();  
  double getElapsedTimeInMicroSec();  

 private:
  double startTimeInMicroSec;  
  double endTimeInMicroSec;    
  int stopped;                 
#ifdef _WIN32
  LARGE_INTEGER frequency;  
  LARGE_INTEGER startCount;
  LARGE_INTEGER endCount;
#else
  timeval startCount;
  timeval endCount;
#endif
};

struct TStruct {
  std::vector<double> records;
  double overall_time;

  TStruct() { overall_time = 0; }

  void push_back(double t) {
    records.push_back(t);
    overall_time += t;
  }
};

extern const Eigen::IOFormat vfmt;
extern const Eigen::IOFormat pyfmt;
typedef Eigen::AngleAxis<FCL_REAL> AngleAxis;
extern const Vec3f UnitX;
extern const Vec3f UnitY;
extern const Vec3f UnitZ;

void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
                 std::vector<Triangle>& triangles);

void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
                 std::vector<Triangle>& triangles);

#ifdef HPP_FCL_HAS_OCTOMAP
fcl::OcTree loadOctreeFile(const std::string& filename,
                           const FCL_REAL& resolution);
#endif

void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform);

void generateRandomTransforms(FCL_REAL extents[6],
                              std::vector<Transform3f>& transforms,
                              std::size_t n);

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);

struct DistanceRes {
  double distance;
  Vec3f p1;
  Vec3f p2;
};

bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
                              void* cdata);

bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
                             void* cdata, FCL_REAL& dist);

std::string getNodeTypeName(NODE_TYPE node_type);

Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);

std::ostream& operator<<(std::ostream& os, const Transform3f& tf);

std::size_t getNbRun(const int& argc, char const* const* argv,
                     std::size_t defaultValue);

void generateEnvironments(std::vector<CollisionObject*>& env,
                          FCL_REAL env_scale, std::size_t n);

void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
                              FCL_REAL env_scale, std::size_t n);

Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid);

}  // namespace fcl

}  // namespace hpp

#endif