Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	test/broadphase_collision_2.cpp	Lines:	155	155	100.0 %
Date:	2024-02-09 12:57:42	Branches:	406	750	54.1 %


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, 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.
 */

/** @author Jia Pan */

#define BOOST_TEST_MODULE BROADPHASE_COLLISION_2
#include <boost/test/included/unit_test.hpp>

#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
#include "hpp/fcl/broadphase/broadphase_SaP.h"
#include "hpp/fcl/broadphase/broadphase_SSaP.h"
#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
#include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
#include "hpp/fcl/broadphase/detail/spatial_hash.h"
#include "utility.h"

#if USE_GOOGLEHASH
#include <sparsehash/sparse_hash_map>
#include <sparsehash/dense_hash_map>
#include <hash_map>
#endif

#include <iostream>
#include <iomanip>

using namespace hpp::fcl;

/// @brief test for broad phase collision and self collision
void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
                                std::size_t query_size,
                                std::size_t num_max_contacts = 1,
                                bool exhaustive = false, bool use_mesh = false);

#if USE_GOOGLEHASH
template <typename U, typename V>
struct GoogleSparseHashTable
    : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
                                     std::equal_to<size_t> > {};

template <typename U, typename V>
struct GoogleDenseHashTable
    : public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
                                    std::equal_to<size_t> > {
  GoogleDenseHashTable()
      : google::dense_hash_map<U, V, std::tr1::hash<size_t>,
                               std::equal_to<size_t> >() {
    this->set_empty_key(nullptr);
  }
};
#endif

/// check broad phase collision for empty collision object set and queries
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) {
#ifdef NDEBUG
  broad_phase_collision_test(2000, 0, 0, 10, false, false);
  broad_phase_collision_test(2000, 0, 1000, 10, false, false);
  broad_phase_collision_test(2000, 100, 0, 10, false, false);

  broad_phase_collision_test(2000, 0, 0, 10, false, true);
  broad_phase_collision_test(2000, 0, 1000, 10, false, true);
  broad_phase_collision_test(2000, 100, 0, 10, false, true);

  broad_phase_collision_test(2000, 0, 0, 10, true, false);
  broad_phase_collision_test(2000, 0, 1000, 10, true, false);
  broad_phase_collision_test(2000, 100, 0, 10, true, false);

  broad_phase_collision_test(2000, 0, 0, 10, true, true);
  broad_phase_collision_test(2000, 0, 1000, 10, true, true);
  broad_phase_collision_test(2000, 100, 0, 10, true, true);
#else
  broad_phase_collision_test(2000, 0, 0, 10, false, false);
  broad_phase_collision_test(2000, 0, 5, 10, false, false);
  broad_phase_collision_test(2000, 2, 0, 10, false, false);

  broad_phase_collision_test(2000, 0, 0, 10, false, true);
  broad_phase_collision_test(2000, 0, 5, 10, false, true);
  broad_phase_collision_test(2000, 2, 0, 10, false, true);

  broad_phase_collision_test(2000, 0, 0, 10, true, false);
  broad_phase_collision_test(2000, 0, 5, 10, true, false);
  broad_phase_collision_test(2000, 2, 0, 10, true, false);

  broad_phase_collision_test(2000, 0, 0, 10, true, true);
  broad_phase_collision_test(2000, 0, 5, 10, true, true);
  broad_phase_collision_test(2000, 2, 0, 10, true, true);
#endif
}

/// check broad phase collision and self collision, only return collision or not
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) {
#ifdef NDEBUG
  broad_phase_collision_test(2000, 100, 1000, 1, false);
  broad_phase_collision_test(2000, 1000, 1000, 1, false);
  broad_phase_collision_test(2000, 100, 1000, 1, true);
  broad_phase_collision_test(2000, 1000, 1000, 1, true);
#else
  broad_phase_collision_test(2000, 10, 100, 1, false);
  broad_phase_collision_test(2000, 100, 100, 1, false);
  broad_phase_collision_test(2000, 10, 100, 1, true);
  broad_phase_collision_test(2000, 100, 100, 1, true);
#endif
}

/// check broad phase collision and self collision, return 10 contacts
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) {
#ifdef NDEBUG
  broad_phase_collision_test(2000, 100, 1000, 10, false);
  broad_phase_collision_test(2000, 1000, 1000, 10, false);
#else
  broad_phase_collision_test(2000, 10, 100, 10, false);
  broad_phase_collision_test(2000, 100, 100, 10, false);
#endif
}

/// check broad phase collision and self collision, return only collision or
/// not, in mesh
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) {
#ifdef NDEBUG
  broad_phase_collision_test(2000, 100, 1000, 1, false, true);
  broad_phase_collision_test(2000, 1000, 1000, 1, false, true);
#else
  broad_phase_collision_test(2000, 2, 5, 1, false, true);
  broad_phase_collision_test(2000, 5, 5, 1, false, true);
#endif
}

/// check broad phase collision and self collision, return 10 contacts, in mesh
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) {
#ifdef NDEBUG
  broad_phase_collision_test(2000, 100, 1000, 10, false, true);
  broad_phase_collision_test(2000, 1000, 1000, 10, false, true);
#else
  broad_phase_collision_test(2000, 2, 5, 10, false, true);
  broad_phase_collision_test(2000, 5, 5, 10, false, true);
#endif
}

/// check broad phase collision and self collision, exhaustive, in mesh
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) {
#ifdef NDEBUG
  broad_phase_collision_test(2000, 100, 1000, 1, true, true);
  broad_phase_collision_test(2000, 1000, 1000, 1, true, true);
#else
  broad_phase_collision_test(2000, 2, 5, 1, true, true);
  broad_phase_collision_test(2000, 5, 5, 1, true, true);
#endif
}

void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
                                std::size_t query_size,
                                std::size_t num_max_contacts, bool exhaustive,
                                bool use_mesh) {
  std::vector<TStruct> ts;
  std::vector<BenchTimer> timers;

  std::vector<CollisionObject*> env;
  if (use_mesh)
    generateEnvironmentsMesh(env, env_scale, env_size);
  else
    generateEnvironments(env, env_scale, env_size);

  std::vector<CollisionObject*> query;
  if (use_mesh)
    generateEnvironmentsMesh(query, env_scale, query_size);
  else
    generateEnvironments(query, env_scale, query_size);

  std::vector<BroadPhaseCollisionManager*> managers;

  managers.push_back(new NaiveCollisionManager());
  managers.push_back(new SSaPCollisionManager());
  managers.push_back(new SaPCollisionManager());
  managers.push_back(new IntervalTreeCollisionManager());

  Vec3f lower_limit, upper_limit;
  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
  // FCL_REAL ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
  FCL_REAL ncell_per_axis = 20;
  FCL_REAL cell_size =
      std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis,
                        (upper_limit[1] - lower_limit[1]) / ncell_per_axis),
               (upper_limit[2] - lower_limit[2]) / ncell_per_axis);
  // managers.push_back(new SpatialHashingCollisionManager(cell_size,
  // lower_limit, upper_limit));
  managers.push_back(new SpatialHashingCollisionManager<detail::SparseHashTable<
                         AABB, CollisionObject*, detail::SpatialHash> >(
      cell_size, lower_limit, upper_limit));
#if USE_GOOGLEHASH
  managers.push_back(
      new SpatialHashingCollisionManager<detail::SparseHashTable<
          AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable> >(
          cell_size, lower_limit, upper_limit));
  managers.push_back(
      new SpatialHashingCollisionManager<detail::SparseHashTable<
          AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable> >(
          cell_size, lower_limit, upper_limit));
#endif
  managers.push_back(new DynamicAABBTreeCollisionManager());

  managers.push_back(new DynamicAABBTreeArrayCollisionManager());

  {
    DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
    m->tree_init_level = 2;
    managers.push_back(m);
  }

  {
    DynamicAABBTreeArrayCollisionManager* m =
        new DynamicAABBTreeArrayCollisionManager();
    m->tree_init_level = 2;
    managers.push_back(m);
  }

  ts.resize(managers.size());
  timers.resize(managers.size());

  for (size_t i = 0; i < managers.size(); ++i) {
    timers[i].start();
    managers[i]->registerObjects(env);
    timers[i].stop();
    ts[i].push_back(timers[i].getElapsedTime());
  }

  for (size_t i = 0; i < managers.size(); ++i) {
    timers[i].start();
    managers[i]->setup();
    timers[i].stop();
    ts[i].push_back(timers[i].getElapsedTime());
  }

  std::vector<CollisionCallBackDefault> callbacks(managers.size());
  for (size_t i = 0; i < managers.size(); ++i) {
    if (exhaustive)
      callbacks[i].data.request.num_max_contacts = 100000;
    else
      callbacks[i].data.request.num_max_contacts = num_max_contacts;
  }

  for (size_t i = 0; i < managers.size(); ++i) {
    timers[i].start();
    managers[i]->collide(&callbacks[i]);
    timers[i].stop();
    ts[i].push_back(timers[i].getElapsedTime());
  }

  for (size_t i = 0; i < managers.size(); ++i)
    std::cout << callbacks[i].data.result.numContacts() << " ";
  std::cout << std::endl;

  if (exhaustive) {
    for (size_t i = 1; i < managers.size(); ++i)
      BOOST_CHECK(callbacks[i].data.result.numContacts() ==
                  callbacks[0].data.result.numContacts());
  } else {
    std::vector<bool> self_res(managers.size());
    for (size_t i = 0; i < self_res.size(); ++i)
      self_res[i] = (callbacks[i].data.result.numContacts() > 0);

    for (size_t i = 1; i < self_res.size(); ++i)
      BOOST_CHECK(self_res[0] == self_res[i]);

    for (size_t i = 1; i < managers.size(); ++i)
      BOOST_CHECK(callbacks[i].data.result.numContacts() ==
                  callbacks[0].data.result.numContacts());
  }

  for (size_t i = 0; i < query.size(); ++i) {
    std::vector<CollisionCallBackDefault> callbacks(managers.size());
    for (size_t j = 0; j < managers.size(); ++j) {
      if (exhaustive)
        callbacks[j].data.request.num_max_contacts = 100000;
      else
        callbacks[j].data.request.num_max_contacts = num_max_contacts;
    }

    for (size_t j = 0; j < managers.size(); ++j) {
      timers[j].start();
      managers[j]->collide(query[i], &callbacks[j]);
      timers[j].stop();
      ts[j].push_back(timers[j].getElapsedTime());
    }

    // for(size_t j = 0; j < managers.size(); ++j)
    //   std::cout << callbacks[i].data.result.numContacts() << " ";
    // std::cout << std::endl;

    if (exhaustive) {
      for (size_t j = 1; j < managers.size(); ++j)
        BOOST_CHECK(callbacks[j].data.result.numContacts() ==
                    callbacks[0].data.result.numContacts());
    } else {
      std::vector<bool> query_res(managers.size());
      for (size_t j = 0; j < query_res.size(); ++j)
        query_res[j] = (callbacks[j].data.result.numContacts() > 0);
      for (size_t j = 1; j < query_res.size(); ++j)
        BOOST_CHECK(query_res[0] == query_res[j]);

      for (size_t j = 1; j < managers.size(); ++j)
        BOOST_CHECK(callbacks[j].data.result.numContacts() ==
                    callbacks[0].data.result.numContacts());
    }
  }

  for (size_t i = 0; i < env.size(); ++i) delete env[i];
  for (size_t i = 0; i < query.size(); ++i) delete query[i];

  for (size_t i = 0; i < managers.size(); ++i) delete managers[i];

  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
  int w = 7;

  std::cout << "collision timing summary" << std::endl;
  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
  std::cout << "register time" << std::endl;
  for (size_t i = 0; i < ts.size(); ++i)
    std::cout << std::setw(w) << ts[i].records[0] << " ";
  std::cout << std::endl;

  std::cout << "setup time" << std::endl;
  for (size_t i = 0; i < ts.size(); ++i)
    std::cout << std::setw(w) << ts[i].records[1] << " ";
  std::cout << std::endl;

  std::cout << "self collision time" << std::endl;
  for (size_t i = 0; i < ts.size(); ++i)
    std::cout << std::setw(w) << ts[i].records[2] << " ";
  std::cout << std::endl;

  std::cout << "collision time" << std::endl;
  for (size_t i = 0; i < ts.size(); ++i) {
    FCL_REAL tmp = 0;
    for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
    std::cout << std::setw(w) << tmp << " ";
  }
  std::cout << std::endl;

  std::cout << "overall time" << std::endl;
  for (size_t i = 0; i < ts.size(); ++i)
    std::cout << std::setw(w) << ts[i].overall_time << " ";
  std::cout << std::endl;
  std::cout << std::endl;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2016, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			/** @author Jia Pan */
37
38			#define BOOST_TEST_MODULE BROADPHASE_COLLISION_2
39			#include <boost/test/included/unit_test.hpp>
40
41			#include "hpp/fcl/broadphase/broadphase_bruteforce.h"
42			#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
43			#include "hpp/fcl/broadphase/broadphase_SaP.h"
44			#include "hpp/fcl/broadphase/broadphase_SSaP.h"
45			#include "hpp/fcl/broadphase/broadphase_interval_tree.h"
46			#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
47			#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
48			#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
49			#include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
50			#include "hpp/fcl/broadphase/detail/spatial_hash.h"
51			#include "utility.h"
52
53			#if USE_GOOGLEHASH
54			#include <sparsehash/sparse_hash_map>
55			#include <sparsehash/dense_hash_map>
56			#include <hash_map>
57			#endif
58
59			#include <iostream>
60			#include <iomanip>
61
62			using namespace hpp::fcl;
63
64			/// @brief test for broad phase collision and self collision
65			void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
66			std::size_t query_size,
67			std::size_t num_max_contacts = 1,
68			bool exhaustive = false, bool use_mesh = false);
69
70			#if USE_GOOGLEHASH
71			template <typename U, typename V>
72			struct GoogleSparseHashTable
73			: public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
74			std::equal_to<size_t> > {};
75
76			template <typename U, typename V>
77			struct GoogleDenseHashTable
78			: public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
79			std::equal_to<size_t> > {
80			GoogleDenseHashTable()
81			: google::dense_hash_map<U, V, std::tr1::hash<size_t>,
82			std::equal_to<size_t> >() {
83			this->set_empty_key(nullptr);
84			}
85			};
86			#endif
87
88			/// check broad phase collision for empty collision object set and queries
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) {
90			#ifdef NDEBUG
91			broad_phase_collision_test(2000, 0, 0, 10, false, false);
92			broad_phase_collision_test(2000, 0, 1000, 10, false, false);
93			broad_phase_collision_test(2000, 100, 0, 10, false, false);
94
95			broad_phase_collision_test(2000, 0, 0, 10, false, true);
96			broad_phase_collision_test(2000, 0, 1000, 10, false, true);
97			broad_phase_collision_test(2000, 100, 0, 10, false, true);
98
99			broad_phase_collision_test(2000, 0, 0, 10, true, false);
100			broad_phase_collision_test(2000, 0, 1000, 10, true, false);
101			broad_phase_collision_test(2000, 100, 0, 10, true, false);
102
103			broad_phase_collision_test(2000, 0, 0, 10, true, true);
104			broad_phase_collision_test(2000, 0, 1000, 10, true, true);
105			broad_phase_collision_test(2000, 100, 0, 10, true, true);
106			#else
107		2	broad_phase_collision_test(2000, 0, 0, 10, false, false);
108		2	broad_phase_collision_test(2000, 0, 5, 10, false, false);
109		2	broad_phase_collision_test(2000, 2, 0, 10, false, false);
110
111		2	broad_phase_collision_test(2000, 0, 0, 10, false, true);
112		2	broad_phase_collision_test(2000, 0, 5, 10, false, true);
113		2	broad_phase_collision_test(2000, 2, 0, 10, false, true);
114
115		2	broad_phase_collision_test(2000, 0, 0, 10, true, false);
116		2	broad_phase_collision_test(2000, 0, 5, 10, true, false);
117		2	broad_phase_collision_test(2000, 2, 0, 10, true, false);
118
119		2	broad_phase_collision_test(2000, 0, 0, 10, true, true);
120		2	broad_phase_collision_test(2000, 0, 5, 10, true, true);
121		2	broad_phase_collision_test(2000, 2, 0, 10, true, true);
122			#endif
123		2	}
124
125			/// check broad phase collision and self collision, only return collision or not
126	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) {
127			#ifdef NDEBUG
128			broad_phase_collision_test(2000, 100, 1000, 1, false);
129			broad_phase_collision_test(2000, 1000, 1000, 1, false);
130			broad_phase_collision_test(2000, 100, 1000, 1, true);
131			broad_phase_collision_test(2000, 1000, 1000, 1, true);
132			#else
133		2	broad_phase_collision_test(2000, 10, 100, 1, false);
134		2	broad_phase_collision_test(2000, 100, 100, 1, false);
135		2	broad_phase_collision_test(2000, 10, 100, 1, true);
136		2	broad_phase_collision_test(2000, 100, 100, 1, true);
137			#endif
138		2	}
139
140			/// check broad phase collision and self collision, return 10 contacts
141	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) {
142			#ifdef NDEBUG
143			broad_phase_collision_test(2000, 100, 1000, 10, false);
144			broad_phase_collision_test(2000, 1000, 1000, 10, false);
145			#else
146		2	broad_phase_collision_test(2000, 10, 100, 10, false);
147		2	broad_phase_collision_test(2000, 100, 100, 10, false);
148			#endif
149		2	}
150
151			/// check broad phase collision and self collision, return only collision or
152			/// not, in mesh
153	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) {
154			#ifdef NDEBUG
155			broad_phase_collision_test(2000, 100, 1000, 1, false, true);
156			broad_phase_collision_test(2000, 1000, 1000, 1, false, true);
157			#else
158		2	broad_phase_collision_test(2000, 2, 5, 1, false, true);
159		2	broad_phase_collision_test(2000, 5, 5, 1, false, true);
160			#endif
161		2	}
162
163			/// check broad phase collision and self collision, return 10 contacts, in mesh
164	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) {
165			#ifdef NDEBUG
166			broad_phase_collision_test(2000, 100, 1000, 10, false, true);
167			broad_phase_collision_test(2000, 1000, 1000, 10, false, true);
168			#else
169		2	broad_phase_collision_test(2000, 2, 5, 10, false, true);
170		2	broad_phase_collision_test(2000, 5, 5, 10, false, true);
171			#endif
172		2	}
173
174			/// check broad phase collision and self collision, exhaustive, in mesh
175	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) {
176			#ifdef NDEBUG
177			broad_phase_collision_test(2000, 100, 1000, 1, true, true);
178			broad_phase_collision_test(2000, 1000, 1000, 1, true, true);
179			#else
180		2	broad_phase_collision_test(2000, 2, 5, 1, true, true);
181		2	broad_phase_collision_test(2000, 5, 5, 1, true, true);
182			#endif
183		2	}
184
185		24	void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size,
186			std::size_t query_size,
187			std::size_t num_max_contacts, bool exhaustive,
188			bool use_mesh) {
189		48	std::vector<TStruct> ts;
190		48	std::vector<BenchTimer> timers;
191
192		48	std::vector<CollisionObject*> env;
193	✓✓	24	if (use_mesh)
194	✓✗	12	generateEnvironmentsMesh(env, env_scale, env_size);
195			else
196	✓✗	12	generateEnvironments(env, env_scale, env_size);
197
198		48	std::vector<CollisionObject*> query;
199	✓✓	24	if (use_mesh)
200	✓✗	12	generateEnvironmentsMesh(query, env_scale, query_size);
201			else
202	✓✗	12	generateEnvironments(query, env_scale, query_size);
203
204		48	std::vector<BroadPhaseCollisionManager*> managers;
205
206	✓✗✓✗ ✓✗	24	managers.push_back(new NaiveCollisionManager());
207	✓✗✓✗ ✓✗	24	managers.push_back(new SSaPCollisionManager());
208	✓✗✓✗ ✓✗	24	managers.push_back(new SaPCollisionManager());
209	✓✗✓✗ ✓✗	24	managers.push_back(new IntervalTreeCollisionManager());
210
211	✓✗✓✗	24	Vec3f lower_limit, upper_limit;
212	✓✗	24	SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
213			// FCL_REAL ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
214		24	FCL_REAL ncell_per_axis = 20;
215			FCL_REAL cell_size =
216	✓✗	24	std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis,
217	✓✗✓✗	24	(upper_limit[1] - lower_limit[1]) / ncell_per_axis),
218	✓✗✓✗ ✓✗	48	(upper_limit[2] - lower_limit[2]) / ncell_per_axis);
219			// managers.push_back(new SpatialHashingCollisionManager(cell_size,
220			// lower_limit, upper_limit));
221	✓✗	24	managers.push_back(new SpatialHashingCollisionManager<detail::SparseHashTable<
222			AABB, CollisionObject*, detail::SpatialHash> >(
223	✓✗✓✗	24	cell_size, lower_limit, upper_limit));
224			#if USE_GOOGLEHASH
225			managers.push_back(
226			new SpatialHashingCollisionManager<detail::SparseHashTable<
227			AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable> >(
228			cell_size, lower_limit, upper_limit));
229			managers.push_back(
230			new SpatialHashingCollisionManager<detail::SparseHashTable<
231			AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable> >(
232			cell_size, lower_limit, upper_limit));
233			#endif
234	✓✗✓✗ ✓✗	24	managers.push_back(new DynamicAABBTreeCollisionManager());
235
236	✓✗✓✗ ✓✗	24	managers.push_back(new DynamicAABBTreeArrayCollisionManager());
237
238			{
239	✓✗✓✗	24	DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
240		24	m->tree_init_level = 2;
241	✓✗	24	managers.push_back(m);
242			}
243
244			{
245			DynamicAABBTreeArrayCollisionManager* m =
246	✓✗✓✗	24	new DynamicAABBTreeArrayCollisionManager();
247		24	m->tree_init_level = 2;
248	✓✗	24	managers.push_back(m);
249			}
250
251	✓✗	24	ts.resize(managers.size());
252	✓✗	24	timers.resize(managers.size());
253
254	✓✓	240	for (size_t i = 0; i < managers.size(); ++i) {
255	✓✗	216	timers[i].start();
256	✓✗	216	managers[i]->registerObjects(env);
257	✓✗	216	timers[i].stop();
258	✓✗✓✗	216	ts[i].push_back(timers[i].getElapsedTime());
259			}
260
261	✓✓	240	for (size_t i = 0; i < managers.size(); ++i) {
262	✓✗	216	timers[i].start();
263	✓✗	216	managers[i]->setup();
264	✓✗	216	timers[i].stop();
265	✓✗✓✗	216	ts[i].push_back(timers[i].getElapsedTime());
266			}
267
268	✓✗	48	std::vector<CollisionCallBackDefault> callbacks(managers.size());
269	✓✓	240	for (size_t i = 0; i < managers.size(); ++i) {
270	✓✓	216	if (exhaustive)
271		90	callbacks[i].data.request.num_max_contacts = 100000;
272			else
273		126	callbacks[i].data.request.num_max_contacts = num_max_contacts;
274			}
275
276	✓✓	240	for (size_t i = 0; i < managers.size(); ++i) {
277	✓✗	216	timers[i].start();
278	✓✗	216	managers[i]->collide(&callbacks[i]);
279	✓✗	216	timers[i].stop();
280	✓✗✓✗	216	ts[i].push_back(timers[i].getElapsedTime());
281			}
282
283	✓✓	240	for (size_t i = 0; i < managers.size(); ++i)
284	✓✗✓✗	216	std::cout << callbacks[i].data.result.numContacts() << " ";
285	✓✗	24	std::cout << std::endl;
286
287	✓✓	24	if (exhaustive) {
288	✓✓	90	for (size_t i = 1; i < managers.size(); ++i)
289	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	80	BOOST_CHECK(callbacks[i].data.result.numContacts() ==
290			callbacks[0].data.result.numContacts());
291			} else {
292	✓✗	28	std::vector<bool> self_res(managers.size());
293	✓✓	140	for (size_t i = 0; i < self_res.size(); ++i)
294		126	self_res[i] = (callbacks[i].data.result.numContacts() > 0);
295
296	✓✓	126	for (size_t i = 1; i < self_res.size(); ++i)
297	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	112	BOOST_CHECK(self_res[0] == self_res[i]);
298
299	✓✓	126	for (size_t i = 1; i < managers.size(); ++i)
300	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	112	BOOST_CHECK(callbacks[i].data.result.numContacts() ==
301			callbacks[0].data.result.numContacts());
302			}
303
304	✓✓	1974	for (size_t i = 0; i < query.size(); ++i) {
305	✓✗	3900	std::vector<CollisionCallBackDefault> callbacks(managers.size());
306	✓✓	19500	for (size_t j = 0; j < managers.size(); ++j) {
307	✓✓	17550	if (exhaustive)
308		5940	callbacks[j].data.request.num_max_contacts = 100000;
309			else
310		11610	callbacks[j].data.request.num_max_contacts = num_max_contacts;
311			}
312
313	✓✓	19500	for (size_t j = 0; j < managers.size(); ++j) {
314	✓✗	17550	timers[j].start();
315	✓✗	17550	managers[j]->collide(query[i], &callbacks[j]);
316	✓✗	17550	timers[j].stop();
317	✓✗✓✗	17550	ts[j].push_back(timers[j].getElapsedTime());
318			}
319
320			// for(size_t j = 0; j < managers.size(); ++j)
321			// std::cout << callbacks[i].data.result.numContacts() << " ";
322			// std::cout << std::endl;
323
324	✓✓	1950	if (exhaustive) {
325	✓✓	5940	for (size_t j = 1; j < managers.size(); ++j)
326	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	5280	BOOST_CHECK(callbacks[j].data.result.numContacts() ==
327			callbacks[0].data.result.numContacts());
328			} else {
329	✓✗	2580	std::vector<bool> query_res(managers.size());
330	✓✓	12900	for (size_t j = 0; j < query_res.size(); ++j)
331		11610	query_res[j] = (callbacks[j].data.result.numContacts() > 0);
332	✓✓	11610	for (size_t j = 1; j < query_res.size(); ++j)
333	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	10320	BOOST_CHECK(query_res[0] == query_res[j]);
334
335	✓✓	11610	for (size_t j = 1; j < managers.size(); ++j)
336	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	10320	BOOST_CHECK(callbacks[j].data.result.numContacts() ==
337			callbacks[0].data.result.numContacts());
338			}
339			}
340
341	✓✓✓✗	1101	for (size_t i = 0; i < env.size(); ++i) delete env[i];
342	✓✓✓✗	1974	for (size_t i = 0; i < query.size(); ++i) delete query[i];
343
344	✓✓✓✗	240	for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
345
346		24	std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
347		24	int w = 7;
348
349	✓✗✓✗	24	std::cout << "collision timing summary" << std::endl;
350	✓✗✓✗ ✓✗✓✗ ✓✗	24	std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
351	✓✗✓✗	24	std::cout << "register time" << std::endl;
352	✓✓	240	for (size_t i = 0; i < ts.size(); ++i)
353	✓✗✓✗ ✓✗	216	std::cout << std::setw(w) << ts[i].records[0] << " ";
354	✓✗	24	std::cout << std::endl;
355
356	✓✗✓✗	24	std::cout << "setup time" << std::endl;
357	✓✓	240	for (size_t i = 0; i < ts.size(); ++i)
358	✓✗✓✗ ✓✗	216	std::cout << std::setw(w) << ts[i].records[1] << " ";
359	✓✗	24	std::cout << std::endl;
360
361	✓✗✓✗	24	std::cout << "self collision time" << std::endl;
362	✓✓	240	for (size_t i = 0; i < ts.size(); ++i)
363	✓✗✓✗ ✓✗	216	std::cout << std::setw(w) << ts[i].records[2] << " ";
364	✓✗	24	std::cout << std::endl;
365
366	✓✗✓✗	24	std::cout << "collision time" << std::endl;
367	✓✓	240	for (size_t i = 0; i < ts.size(); ++i) {
368		216	FCL_REAL tmp = 0;
369	✓✓	17766	for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
370	✓✗✓✗ ✓✗	216	std::cout << std::setw(w) << tmp << " ";
371			}
372	✓✗	24	std::cout << std::endl;
373
374	✓✗✓✗	24	std::cout << "overall time" << std::endl;
375	✓✓	240	for (size_t i = 0; i < ts.size(); ++i)
376	✓✗✓✗ ✓✗	216	std::cout << std::setw(w) << ts[i].overall_time << " ";
377	✓✗	24	std::cout << std::endl;
378	✓✗	24	std::cout << std::endl;
379		24	}