Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h	Lines:	136	224	60.7 %
Date:	2024-02-09 12:57:42	Branches:	154	388	39.7 %


/*
 * 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 */

#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H

#include "hpp/fcl/broadphase/broadphase_spatialhash.h"

namespace hpp {
namespace fcl {

//==============================================================================
template <typename HashTable>
SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
    FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max,
    unsigned int default_table_size)
    : scene_limit(AABB(scene_min, scene_max)),
      hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
  hash_table->init(default_table_size);
}

//==============================================================================
template <typename HashTable>
SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
  delete hash_table;
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::registerObject(
    CollisionObject* obj) {
  objs.push_back(obj);

  const AABB& obj_aabb = obj->getAABB();
  AABB overlap_aabb;

  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
    if (!scene_limit.contain(obj_aabb))
      objs_partially_penetrating_scene_limit.push_back(obj);

    hash_table->insert(overlap_aabb, obj);
  } else {
    objs_outside_scene_limit.push_back(obj);
  }

  obj_aabb_map[obj] = obj_aabb;
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::unregisterObject(
    CollisionObject* obj) {
  objs.remove(obj);

  const AABB& obj_aabb = obj->getAABB();
  AABB overlap_aabb;

  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
    if (!scene_limit.contain(obj_aabb))
      objs_partially_penetrating_scene_limit.remove(obj);

    hash_table->remove(overlap_aabb, obj);
  } else {
    objs_outside_scene_limit.remove(obj);
  }

  obj_aabb_map.erase(obj);
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::setup() {
  // Do nothing
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update() {
  hash_table->clear();
  objs_partially_penetrating_scene_limit.clear();
  objs_outside_scene_limit.clear();

  for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
    CollisionObject* obj = *it;
    const AABB& obj_aabb = obj->getAABB();
    AABB overlap_aabb;

    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
      if (!scene_limit.contain(obj_aabb))
        objs_partially_penetrating_scene_limit.push_back(obj);

      hash_table->insert(overlap_aabb, obj);
    } else {
      objs_outside_scene_limit.push_back(obj);
    }

    obj_aabb_map[obj] = obj_aabb;
  }
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update(
    CollisionObject* updated_obj) {
  const AABB& new_aabb = updated_obj->getAABB();
  const AABB& old_aabb = obj_aabb_map[updated_obj];

  AABB old_overlap_aabb;
  const auto is_old_aabb_overlapping =
      scene_limit.overlap(old_aabb, old_overlap_aabb);
  if (is_old_aabb_overlapping)
    hash_table->remove(old_overlap_aabb, updated_obj);

  AABB new_overlap_aabb;
  const auto is_new_aabb_overlapping =
      scene_limit.overlap(new_aabb, new_overlap_aabb);
  if (is_new_aabb_overlapping)
    hash_table->insert(new_overlap_aabb, updated_obj);

  ObjectStatus old_status;
  if (is_old_aabb_overlapping) {
    if (scene_limit.contain(old_aabb))
      old_status = Inside;
    else
      old_status = PartiallyPenetrating;
  } else {
    old_status = Outside;
  }

  if (is_new_aabb_overlapping) {
    if (scene_limit.contain(new_aabb)) {
      if (old_status == PartiallyPenetrating) {
        // Status change: PartiallyPenetrating --> Inside
        // Required action(s):
        // - remove object from "objs_partially_penetrating_scene_limit"

        auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
                                 objs_partially_penetrating_scene_limit.end(),
                                 updated_obj);
        objs_partially_penetrating_scene_limit.erase(find_it);
      } else if (old_status == Outside) {
        // Status change: Outside --> Inside
        // Required action(s):
        // - remove object from "objs_outside_scene_limit"

        auto find_it = std::find(objs_outside_scene_limit.begin(),
                                 objs_outside_scene_limit.end(), updated_obj);
        objs_outside_scene_limit.erase(find_it);
      }
    } else {
      if (old_status == Inside) {
        // Status change: Inside --> PartiallyPenetrating
        // Required action(s):
        // - add object to "objs_partially_penetrating_scene_limit"

        objs_partially_penetrating_scene_limit.push_back(updated_obj);
      } else if (old_status == Outside) {
        // Status change: Outside --> PartiallyPenetrating
        // Required action(s):
        // - remove object from "objs_outside_scene_limit"
        // - add object to "objs_partially_penetrating_scene_limit"

        auto find_it = std::find(objs_outside_scene_limit.begin(),
                                 objs_outside_scene_limit.end(), updated_obj);
        objs_outside_scene_limit.erase(find_it);

        objs_partially_penetrating_scene_limit.push_back(updated_obj);
      }
    }
  } else {
    if (old_status == Inside) {
      // Status change: Inside --> Outside
      // Required action(s):
      // - add object to "objs_outside_scene_limit"

      objs_outside_scene_limit.push_back(updated_obj);
    } else if (old_status == PartiallyPenetrating) {
      // Status change: PartiallyPenetrating --> Outside
      // Required action(s):
      // - remove object from "objs_partially_penetrating_scene_limit"
      // - add object to "objs_outside_scene_limit"

      auto find_it =
          std::find(objs_partially_penetrating_scene_limit.begin(),
                    objs_partially_penetrating_scene_limit.end(), updated_obj);
      objs_partially_penetrating_scene_limit.erase(find_it);

      objs_outside_scene_limit.push_back(updated_obj);
    }
  }

  obj_aabb_map[updated_obj] = new_aabb;
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update(
    const std::vector<CollisionObject*>& updated_objs) {
  for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::clear() {
  objs.clear();
  hash_table->clear();
  objs_outside_scene_limit.clear();
  obj_aabb_map.clear();
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::getObjects(
    std::vector<CollisionObject*>& objs_) const {
  objs_.resize(objs.size());
  std::copy(objs.begin(), objs.end(), objs_.begin());
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(
    CollisionObject* obj, CollisionCallBackBase* callback) const {
  if (size() == 0) return;
  collide_(obj, callback);
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
    CollisionObject* obj, DistanceCallBackBase* callback) const {
  if (size() == 0) return;
  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
  distance_(obj, callback, min_dist);
}

//==============================================================================
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::collide_(
    CollisionObject* obj, CollisionCallBackBase* callback) const {
  const auto& obj_aabb = obj->getAABB();
  AABB overlap_aabb;

  if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
    const auto query_result = hash_table->query(overlap_aabb);
    for (const auto& obj2 : query_result) {
      if (obj == obj2) continue;

      if ((*callback)(obj, obj2)) return true;
    }

    if (!scene_limit.contain(obj_aabb)) {
      for (const auto& obj2 : objs_outside_scene_limit) {
        if (obj == obj2) continue;

        if ((*callback)(obj, obj2)) return true;
      }
    }
  } else {
    for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
      if (obj == obj2) continue;

      if ((*callback)(obj, obj2)) return true;
    }

    for (const auto& obj2 : objs_outside_scene_limit) {
      if (obj == obj2) continue;

      if ((*callback)(obj, obj2)) return true;
    }
  }

  return false;
}

//==============================================================================
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::distance_(
    CollisionObject* obj, DistanceCallBackBase* callback,
    FCL_REAL& min_dist) const {
  auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
  auto aabb = obj->getAABB();
  if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
    Vec3f min_dist_delta(min_dist, min_dist, min_dist);
    aabb.expand(min_dist_delta);
  }

  AABB overlap_aabb;

  auto status = 1;
  FCL_REAL old_min_distance;

  while (1) {
    old_min_distance = min_dist;

    if (scene_limit.overlap(aabb, overlap_aabb)) {
      if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
                                  callback, min_dist)) {
        return true;
      }

      if (!scene_limit.contain(aabb)) {
        if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
                                    min_dist)) {
          return true;
        }
      }
    } else {
      if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
                                  callback, min_dist)) {
        return true;
      }

      if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
                                  min_dist)) {
        return true;
      }
    }

    if (status == 1) {
      if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) {
        break;
      } else {
        if (min_dist < old_min_distance) {
          Vec3f min_dist_delta(min_dist, min_dist, min_dist);
          aabb = AABB(obj->getAABB(), min_dist_delta);
          status = 0;
        } else {
          if (aabb == obj->getAABB())
            aabb.expand(delta);
          else
            aabb.expand(obj->getAABB(), 2.0);
        }
      }
    } else if (status == 0) {
      break;
    }
  }

  return false;
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(
    CollisionCallBackBase* callback) const {
  if (size() == 0) return;

  for (const auto& obj1 : objs) {
    const auto& obj_aabb = obj1->getAABB();
    AABB overlap_aabb;

    if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
      auto query_result = hash_table->query(overlap_aabb);
      for (const auto& obj2 : query_result) {
        if (obj1 < obj2) {
          if ((*callback)(obj1, obj2)) return;
        }
      }

      if (!scene_limit.contain(obj_aabb)) {
        for (const auto& obj2 : objs_outside_scene_limit) {
          if (obj1 < obj2) {
            if ((*callback)(obj1, obj2)) return;
          }
        }
      }
    } else {
      for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
        if (obj1 < obj2) {
          if ((*callback)(obj1, obj2)) return;
        }
      }

      for (const auto& obj2 : objs_outside_scene_limit) {
        if (obj1 < obj2) {
          if ((*callback)(obj1, obj2)) return;
        }
      }
    }
  }
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
    DistanceCallBackBase* callback) const {
  if (size() == 0) return;

  this->enable_tested_set_ = true;
  this->tested_set.clear();

  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();

  for (const auto& obj : objs) {
    if (distance_(obj, callback, min_dist)) break;
  }

  this->enable_tested_set_ = false;
  this->tested_set.clear();
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(
    BroadPhaseCollisionManager* other_manager_,
    CollisionCallBackBase* callback) const {
  auto* other_manager =
      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);

  if ((size() == 0) || (other_manager->size() == 0)) return;

  if (this == other_manager) {
    collide(callback);
    return;
  }

  if (this->size() < other_manager->size()) {
    for (const auto& obj : objs) {
      if (other_manager->collide_(obj, callback)) return;
    }
  } else {
    for (const auto& obj : other_manager->objs) {
      if (collide_(obj, callback)) return;
    }
  }
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
    BroadPhaseCollisionManager* other_manager_,
    DistanceCallBackBase* callback) const {
  auto* other_manager =
      static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);

  if ((size() == 0) || (other_manager->size() == 0)) return;

  if (this == other_manager) {
    distance(callback);
    return;
  }

  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();

  if (this->size() < other_manager->size()) {
    for (const auto& obj : objs)
      if (other_manager->distance_(obj, callback, min_dist)) return;
  } else {
    for (const auto& obj : other_manager->objs)
      if (distance_(obj, callback, min_dist)) return;
  }
}

//==============================================================================
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::empty() const {
  return objs.empty();
}

//==============================================================================
template <typename HashTable>
size_t SpatialHashingCollisionManager<HashTable>::size() const {
  return objs.size();
}

//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::computeBound(
    std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u) {
  AABB bound;
  for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();

  l = bound.min_;
  u = bound.max_;
}

//==============================================================================
template <typename HashTable>
template <typename Container>
bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
    CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
    FCL_REAL& min_dist) const {
  for (auto& obj2 : objs) {
    if (obj == obj2) continue;

    if (!this->enable_tested_set_) {
      if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
        if ((*callback)(obj, obj2, min_dist)) return true;
      }
    } else {
      if (!this->inTestedSet(obj, obj2)) {
        if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
          if ((*callback)(obj, obj2, min_dist)) return true;
        }

        this->insertTestedSet(obj, obj2);
      }
    }
  }

  return false;
}

}  // namespace fcl

}  // namespace hpp

#endif


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			#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
39			#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
40
41			#include "hpp/fcl/broadphase/broadphase_spatialhash.h"
42
43			namespace hpp {
44			namespace fcl {
45
46			//==============================================================================
47			template <typename HashTable>
48		51	SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
49			FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max,
50			unsigned int default_table_size)
51			: scene_limit(AABB(scene_min, scene_max)),
52	✓✗✓✗ ✓✗✓✗	51	hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
53	✗✗	51	hash_table->init(default_table_size);
54		51	}
55
56			//==============================================================================
57			template <typename HashTable>
58		200	SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
59		100	delete hash_table;
60	✓✗	300	}
61
62			//==============================================================================
63			template <typename HashTable>
64		12671	void SpatialHashingCollisionManager<HashTable>::registerObject(
65			CollisionObject* obj) {
66	✓✗	12671	objs.push_back(obj);
67
68		12671	const AABB& obj_aabb = obj->getAABB();
69	✓✗	12671	AABB overlap_aabb;
70
71	✓✗✓✗	12671	if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
72	✓✗✗✓	12671	if (!scene_limit.contain(obj_aabb))
73			objs_partially_penetrating_scene_limit.push_back(obj);
74
75	✓✗✓✗	12671	hash_table->insert(overlap_aabb, obj);
76			} else {
77			objs_outside_scene_limit.push_back(obj);
78			}
79
80	✓✗✓✗	12671	obj_aabb_map[obj] = obj_aabb;
81		12671	}
82
83			//==============================================================================
84			template <typename HashTable>
85			void SpatialHashingCollisionManager<HashTable>::unregisterObject(
86			CollisionObject* obj) {
87			objs.remove(obj);
88
89			const AABB& obj_aabb = obj->getAABB();
90			AABB overlap_aabb;
91
92			if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
93			if (!scene_limit.contain(obj_aabb))
94			objs_partially_penetrating_scene_limit.remove(obj);
95
96			hash_table->remove(overlap_aabb, obj);
97			} else {
98			objs_outside_scene_limit.remove(obj);
99			}
100
101			obj_aabb_map.erase(obj);
102			}
103
104			//==============================================================================
105			template <typename HashTable>
106		51	void SpatialHashingCollisionManager<HashTable>::setup() {
107			// Do nothing
108		51	}
109
110			//==============================================================================
111			template <typename HashTable>
112		13	void SpatialHashingCollisionManager<HashTable>::update() {
113		13	hash_table->clear();
114		13	objs_partially_penetrating_scene_limit.clear();
115		13	objs_outside_scene_limit.clear();
116
117	✓✓	1357	for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
118		1344	CollisionObject* obj = *it;
119		1344	const AABB& obj_aabb = obj->getAABB();
120	✓✗	1344	AABB overlap_aabb;
121
122	✓✗✓✓	1344	if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
123	✓✗✓✓	1255	if (!scene_limit.contain(obj_aabb))
124	✓✗	55	objs_partially_penetrating_scene_limit.push_back(obj);
125
126	✓✗✓✗	1255	hash_table->insert(overlap_aabb, obj);
127			} else {
128	✓✗	89	objs_outside_scene_limit.push_back(obj);
129			}
130
131	✓✗✓✗	1344	obj_aabb_map[obj] = obj_aabb;
132			}
133		13	}
134
135			//==============================================================================
136			template <typename HashTable>
137			void SpatialHashingCollisionManager<HashTable>::update(
138			CollisionObject* updated_obj) {
139			const AABB& new_aabb = updated_obj->getAABB();
140			const AABB& old_aabb = obj_aabb_map[updated_obj];
141
142			AABB old_overlap_aabb;
143			const auto is_old_aabb_overlapping =
144			scene_limit.overlap(old_aabb, old_overlap_aabb);
145			if (is_old_aabb_overlapping)
146			hash_table->remove(old_overlap_aabb, updated_obj);
147
148			AABB new_overlap_aabb;
149			const auto is_new_aabb_overlapping =
150			scene_limit.overlap(new_aabb, new_overlap_aabb);
151			if (is_new_aabb_overlapping)
152			hash_table->insert(new_overlap_aabb, updated_obj);
153
154			ObjectStatus old_status;
155			if (is_old_aabb_overlapping) {
156			if (scene_limit.contain(old_aabb))
157			old_status = Inside;
158			else
159			old_status = PartiallyPenetrating;
160			} else {
161			old_status = Outside;
162			}
163
164			if (is_new_aabb_overlapping) {
165			if (scene_limit.contain(new_aabb)) {
166			if (old_status == PartiallyPenetrating) {
167			// Status change: PartiallyPenetrating --> Inside
168			// Required action(s):
169			// - remove object from "objs_partially_penetrating_scene_limit"
170
171			auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
172			objs_partially_penetrating_scene_limit.end(),
173			updated_obj);
174			objs_partially_penetrating_scene_limit.erase(find_it);
175			} else if (old_status == Outside) {
176			// Status change: Outside --> Inside
177			// Required action(s):
178			// - remove object from "objs_outside_scene_limit"
179
180			auto find_it = std::find(objs_outside_scene_limit.begin(),
181			objs_outside_scene_limit.end(), updated_obj);
182			objs_outside_scene_limit.erase(find_it);
183			}
184			} else {
185			if (old_status == Inside) {
186			// Status change: Inside --> PartiallyPenetrating
187			// Required action(s):
188			// - add object to "objs_partially_penetrating_scene_limit"
189
190			objs_partially_penetrating_scene_limit.push_back(updated_obj);
191			} else if (old_status == Outside) {
192			// Status change: Outside --> PartiallyPenetrating
193			// Required action(s):
194			// - remove object from "objs_outside_scene_limit"
195			// - add object to "objs_partially_penetrating_scene_limit"
196
197			auto find_it = std::find(objs_outside_scene_limit.begin(),
198			objs_outside_scene_limit.end(), updated_obj);
199			objs_outside_scene_limit.erase(find_it);
200
201			objs_partially_penetrating_scene_limit.push_back(updated_obj);
202			}
203			}
204			} else {
205			if (old_status == Inside) {
206			// Status change: Inside --> Outside
207			// Required action(s):
208			// - add object to "objs_outside_scene_limit"
209
210			objs_outside_scene_limit.push_back(updated_obj);
211			} else if (old_status == PartiallyPenetrating) {
212			// Status change: PartiallyPenetrating --> Outside
213			// Required action(s):
214			// - remove object from "objs_partially_penetrating_scene_limit"
215			// - add object to "objs_outside_scene_limit"
216
217			auto find_it =
218			std::find(objs_partially_penetrating_scene_limit.begin(),
219			objs_partially_penetrating_scene_limit.end(), updated_obj);
220			objs_partially_penetrating_scene_limit.erase(find_it);
221
222			objs_outside_scene_limit.push_back(updated_obj);
223			}
224			}
225
226			obj_aabb_map[updated_obj] = new_aabb;
227			}
228
229			//==============================================================================
230			template <typename HashTable>
231			void SpatialHashingCollisionManager<HashTable>::update(
232			const std::vector<CollisionObject*>& updated_objs) {
233			for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
234			}
235
236			//==============================================================================
237			template <typename HashTable>
238			void SpatialHashingCollisionManager<HashTable>::clear() {
239			objs.clear();
240			hash_table->clear();
241			objs_outside_scene_limit.clear();
242			obj_aabb_map.clear();
243			}
244
245			//==============================================================================
246			template <typename HashTable>
247			void SpatialHashingCollisionManager<HashTable>::getObjects(
248			std::vector<CollisionObject*>& objs_) const {
249			objs_.resize(objs.size());
250			std::copy(objs.begin(), objs.end(), objs_.begin());
251			}
252
253			//==============================================================================
254			template <typename HashTable>
255		3822	void SpatialHashingCollisionManager<HashTable>::collide(
256			CollisionObject* obj, CollisionCallBackBase* callback) const {
257	✓✓	3822	if (size() == 0) return;
258		3762	collide_(obj, callback);
259			}
260
261			//==============================================================================
262			template <typename HashTable>
263		80	void SpatialHashingCollisionManager<HashTable>::distance(
264			CollisionObject* obj, DistanceCallBackBase* callback) const {
265	✓✗✗✓	80	if (size() == 0) return;
266		80	FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
267	✓✗	80	distance_(obj, callback, min_dist);
268			}
269
270			//==============================================================================
271			template <typename HashTable>
272		3762	bool SpatialHashingCollisionManager<HashTable>::collide_(
273			CollisionObject* obj, CollisionCallBackBase* callback) const {
274		3762	const auto& obj_aabb = obj->getAABB();
275	✓✗	3762	AABB overlap_aabb;
276
277	✓✗✓✓	3762	if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
278	✓✗✓✗	3453	const auto query_result = hash_table->query(overlap_aabb);
279	✓✓	3642	for (const auto& obj2 : query_result) {
280	✗✓	189	if (obj == obj2) continue;
281
282	✓✗✗✓	189	if ((*callback)(obj, obj2)) return true;
283			}
284
285	✓✗✓✓	3453	if (!scene_limit.contain(obj_aabb)) {
286	✓✓	851	for (const auto& obj2 : objs_outside_scene_limit) {
287	✗✓	711	if (obj == obj2) continue;
288
289	✓✗✗✓	711	if ((*callback)(obj, obj2)) return true;
290			}
291			}
292			} else {
293	✓✓	412	for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
294	✗✓	103	if (obj == obj2) continue;
295
296	✓✗✗✓	103	if ((*callback)(obj, obj2)) return true;
297			}
298
299	✓✓	774	for (const auto& obj2 : objs_outside_scene_limit) {
300	✗✓	465	if (obj == obj2) continue;
301
302	✓✗✗✓	465	if ((*callback)(obj, obj2)) return true;
303			}
304			}
305
306		3762	return false;
307			}
308
309			//==============================================================================
310			template <typename HashTable>
311		3070	bool SpatialHashingCollisionManager<HashTable>::distance_(
312			CollisionObject* obj, DistanceCallBackBase* callback,
313			FCL_REAL& min_dist) const {
314	✓✗✓✗	3070	auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
315	✓✗	3070	auto aabb = obj->getAABB();
316	✓✓	3070	if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
317	✓✗	2984	Vec3f min_dist_delta(min_dist, min_dist, min_dist);
318	✓✗	2984	aabb.expand(min_dist_delta);
319			}
320
321	✓✗	3070	AABB overlap_aabb;
322
323		3070	auto status = 1;
324			FCL_REAL old_min_distance;
325
326		313	while (1) {
327		3383	old_min_distance = min_dist;
328
329	✓✗✓✓	3383	if (scene_limit.overlap(aabb, overlap_aabb)) {
330	✓✗✓✗ ✓✗✗✓	3378	if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
331			callback, min_dist)) {
332			return true;
333			}
334
335	✓✗✓✓	3378	if (!scene_limit.contain(aabb)) {
336	✓✗✗✓	3092	if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
337			min_dist)) {
338			return true;
339			}
340			}
341			} else {
342	✓✗✗✓	5	if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
343			callback, min_dist)) {
344			return true;
345			}
346
347	✓✗✗✓	5	if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
348			min_dist)) {
349			return true;
350			}
351			}
352
353	✓✓	3383	if (status == 1) {
354	✓✓	3297	if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) {
355		2984	break;
356			} else {
357	✓✓	313	if (min_dist < old_min_distance) {
358	✓✗	86	Vec3f min_dist_delta(min_dist, min_dist, min_dist);
359	✓✗✓✗	86	aabb = AABB(obj->getAABB(), min_dist_delta);
360		86	status = 0;
361			} else {
362	✓✗✓✓	227	if (aabb == obj->getAABB())
363	✓✗✓✗	48	aabb.expand(delta);
364			else
365	✓✗	179	aabb.expand(obj->getAABB(), 2.0);
366			}
367			}
368	✓✗	86	} else if (status == 0) {
369		86	break;
370			}
371			}
372
373		3070	return false;
374			}
375
376			//==============================================================================
377			template <typename HashTable>
378		37	void SpatialHashingCollisionManager<HashTable>::collide(
379			CollisionCallBackBase* callback) const {
380	✓✓	37	if (size() == 0) return;
381
382	✓✓	2338	for (const auto& obj1 : objs) {
383		2310	const auto& obj_aabb = obj1->getAABB();
384	✓✗	2310	AABB overlap_aabb;
385
386	✓✗✓✓	2310	if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
387	✓✗✓✗	2221	auto query_result = hash_table->query(overlap_aabb);
388	✓✓	4663	for (const auto& obj2 : query_result) {
389	✓✓	2443	if (obj1 < obj2) {
390	✓✗✓✓	112	if ((*callback)(obj1, obj2)) return;
391			}
392			}
393
394	✓✗✓✓	2220	if (!scene_limit.contain(obj_aabb)) {
395	✓✓	794	for (const auto& obj2 : objs_outside_scene_limit) {
396	✓✓	739	if (obj1 < obj2) {
397	✓✗✗✓	365	if ((*callback)(obj1, obj2)) return;
398			}
399			}
400			}
401			} else {
402	✓✓	828	for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
403	✓✓	739	if (obj1 < obj2) {
404	✓✗✗✓	374	if ((*callback)(obj1, obj2)) return;
405			}
406			}
407
408	✓✓	1340	for (const auto& obj2 : objs_outside_scene_limit) {
409	✓✓	1251	if (obj1 < obj2) {
410	✓✗✗✓	581	if ((*callback)(obj1, obj2)) return;
411			}
412			}
413			}
414			}
415			}
416
417			//==============================================================================
418			template <typename HashTable>
419		6	void SpatialHashingCollisionManager<HashTable>::distance(
420			DistanceCallBackBase* callback) const {
421	✓✗✗✓	6	if (size() == 0) return;
422
423		6	this->enable_tested_set_ = true;
424		6	this->tested_set.clear();
425
426		6	FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
427
428	✓✓	2996	for (const auto& obj : objs) {
429	✓✗✗✓	2990	if (distance_(obj, callback, min_dist)) break;
430			}
431
432		6	this->enable_tested_set_ = false;
433		6	this->tested_set.clear();
434			}
435
436			//==============================================================================
437			template <typename HashTable>
438			void SpatialHashingCollisionManager<HashTable>::collide(
439			BroadPhaseCollisionManager* other_manager_,
440			CollisionCallBackBase* callback) const {
441			auto* other_manager =
442			static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
443
444			if ((size() == 0) \|\| (other_manager->size() == 0)) return;
445
446			if (this == other_manager) {
447			collide(callback);
448			return;
449			}
450
451			if (this->size() < other_manager->size()) {
452			for (const auto& obj : objs) {
453			if (other_manager->collide_(obj, callback)) return;
454			}
455			} else {
456			for (const auto& obj : other_manager->objs) {
457			if (collide_(obj, callback)) return;
458			}
459			}
460			}
461
462			//==============================================================================
463			template <typename HashTable>
464			void SpatialHashingCollisionManager<HashTable>::distance(
465			BroadPhaseCollisionManager* other_manager_,
466			DistanceCallBackBase* callback) const {
467			auto* other_manager =
468			static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
469
470			if ((size() == 0) \|\| (other_manager->size() == 0)) return;
471
472			if (this == other_manager) {
473			distance(callback);
474			return;
475			}
476
477			FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
478
479			if (this->size() < other_manager->size()) {
480			for (const auto& obj : objs)
481			if (other_manager->distance_(obj, callback, min_dist)) return;
482			} else {
483			for (const auto& obj : other_manager->objs)
484			if (distance_(obj, callback, min_dist)) return;
485			}
486			}
487
488			//==============================================================================
489			template <typename HashTable>
490			bool SpatialHashingCollisionManager<HashTable>::empty() const {
491			return objs.empty();
492			}
493
494			//==============================================================================
495			template <typename HashTable>
496		3945	size_t SpatialHashingCollisionManager<HashTable>::size() const {
497		3945	return objs.size();
498			}
499
500			//==============================================================================
501			template <typename HashTable>
502		51	void SpatialHashingCollisionManager<HashTable>::computeBound(
503			std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u) {
504	✓✗	51	AABB bound;
505	✓✓✓✗	12722	for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
506
507	✓✗	51	l = bound.min_;
508	✓✗	51	u = bound.max_;
509		51	}
510
511			//==============================================================================
512			template <typename HashTable>
513			template <typename Container>
514		6480	bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
515			CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
516			FCL_REAL& min_dist) const {
517		15302	for (auto& obj2 : objs) {
518		8822	if (obj == obj2) continue;
519
520		5794	if (!this->enable_tested_set_) {
521		1322	if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
522		716	if ((*callback)(obj, obj2, min_dist)) return true;
523			}
524			} else {
525		4472	if (!this->inTestedSet(obj, obj2)) {
526		2236	if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
527		475	if ((*callback)(obj, obj2, min_dist)) return true;
528			}
529
530		2236	this->insertTestedSet(obj, obj2);
531			}
532			}
533			}
534
535		6480	return false;
536			}
537
538			} // namespace fcl
539
540			} // namespace hpp
541
542			#endif