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_COAL_UTILITY_H
#define TEST_COAL_UTILITY_H
 
#include "coal/math/transform.h"
#include "coal/collision_data.h"
#include "coal/collision_object.h"
#include "coal/broadphase/default_broadphase_callbacks.h"
#include "coal/shape/convex.h"
 
#ifdef COAL_HAS_OCTOMAP
#include "coal/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 at precision " \
                          << precision << " [\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 at precision " \
                          << precision << " [\n"                             \
                          << (Va) << "\n!=\n"                                \
                          << (Vb) << "\n]")
 
#define CoalScalar_IS_APPROX(Va, Vb, precision)                              \
  BOOST_CHECK_MESSAGE(std::abs((Va) - (Vb)) < precision,                     \
                      "check " #Va ".isApprox(" #Vb ") failed at precision " \
                          << precision << " [\n"                             \
                          << Va << "\n!=\n"                                  \
                          << Vb << "\n]")
 
namespace octomap {
#ifdef COAL_HAS_OCTOMAP
typedef coal::shared_ptr<octomap::OcTree> OcTreePtr_t;
#endif
}  // namespace octomap
namespace coal {
 
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<CoalScalar> AngleAxis;
extern const Vec3s UnitX;
extern const Vec3s UnitY;
extern const Vec3s UnitZ;
 
void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
                 std::vector<Triangle>& triangles);
 
void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
                 std::vector<Triangle>& triangles);
 
#ifdef COAL_HAS_OCTOMAP
coal::OcTree loadOctreeFile(const std::string& filename,
                            const CoalScalar& resolution);
#endif
 
void generateRandomTransform(CoalScalar extents[6], Transform3s& transform);
 
void generateRandomTransforms(CoalScalar extents[6],
                              std::vector<Transform3s>& transforms,
                              std::size_t n);
 
void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
                              CoalScalar delta_rot,
                              std::vector<Transform3s>& transforms,
                              std::vector<Transform3s>& transforms2,
                              std::size_t n);
 
struct DistanceRes {
  double distance;
  Vec3s p1;
  Vec3s p2;
};
 
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
                              void* cdata);
 
bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
                             void* cdata, CoalScalar& dist);
 
std::string getNodeTypeName(NODE_TYPE node_type);
 
Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z);
 
std::ostream& operator<<(std::ostream& os, const Transform3s& tf);
 
std::size_t getNbRun(const int& argc, char const* const* argv,
                     std::size_t defaultValue);
 
void generateEnvironments(std::vector<CollisionObject*>& env,
                          CoalScalar env_scale, std::size_t n);
 
void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
                              CoalScalar env_scale, std::size_t n);
 
Convex<Quadrilateral> buildBox(CoalScalar l, CoalScalar w, CoalScalar d);
 
Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid);
 
Box makeRandomBox(CoalScalar min_size, CoalScalar max_size);
 
Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size);
 
Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size);
 
Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
                          std::array<CoalScalar, 2> max_size);
 
Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
                    std::array<CoalScalar, 2> max_size);
 
Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
                            std::array<CoalScalar, 2> max_size);
 
Convex<Triangle> makeRandomConvex(CoalScalar min_size, CoalScalar max_size);
 
Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size);
 
Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size);
 
std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type);
 
}  // namespace coal
 
#endif