collision_node_asserts.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE COAL_COLLISION_NODE_ASSERT
 
#include <boost/test/included/unit_test.hpp>
#include <boost/math/constants/constants.hpp>
#include "coal/BVH/BVH_model.h"
#include "coal/collision.h"
 
using namespace coal;
 
constexpr CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 
double DegToRad(const double& deg) {
  static double degToRad = pi / 180.;
  return deg * degToRad;
}
std::vector<Vec3s> dirs{Vec3s::UnitZ(),  -Vec3s::UnitZ(), Vec3s::UnitY(),
                        -Vec3s::UnitY(), Vec3s::UnitX(),  -Vec3s::UnitX()};
 
BOOST_AUTO_TEST_CASE(TestTriangles) {
  std::vector<Vec3s> triVertices{Vec3s(1, 0, 0), Vec3s(1, 1, 0),
                                 Vec3s(0, 1, 0)};
  std::vector<Triangle> triangle{{0, 1, 2}};
 
  BVHModel<OBBRSS> tri1{};
  BVHModel<OBBRSS> tri2{};
 
  tri1.beginModel();
  tri1.addSubModel(triVertices, triangle);
  tri1.endModel();
 
  tri2.beginModel();
  tri2.addSubModel(triVertices, triangle);
  tri2.endModel();
 
  CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
 
  ComputeCollision compute(&tri1, &tri2);
 
  Transform3s tri1Tf{};
  Transform3s tri2Tf{};
 
  for (int i = 0; i < 360; i += 30) {
    for (int j = 0; j < 180; j += 30) {
      for (int k = 0; k < 180; k += 30) {
        tri1Tf.setQuatRotation(
            Eigen::AngleAxis<double>(0., Vec3s::UnitZ()) *
            Eigen::AngleAxis<double>(DegToRad(k), Vec3s::UnitY()));
        tri2Tf.setQuatRotation(
            Eigen::AngleAxis<double>(DegToRad(i), Vec3s::UnitZ()) *
            Eigen::AngleAxis<double>(DegToRad(j), Vec3s::UnitY()));
        CollisionResult result;
 
        BOOST_CHECK_NO_THROW(compute(tri2Tf, tri1Tf, request, result));
      }
    }
  }
}