Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/broadphase/broadphase_SSaP.cpp	Lines:	187	253	73.9 %
Date:	2024-02-09 12:57:42	Branches:	175	352	49.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 */

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

namespace hpp {
namespace fcl {

/** @brief Functor sorting objects according to the AABB lower x bound */
struct SortByXLow {
  bool operator()(const CollisionObject* a, const CollisionObject* b) const {
    if (a->getAABB().min_[0] < b->getAABB().min_[0]) return true;
    return false;
  }
};

/** @brief Functor sorting objects according to the AABB lower y bound */
struct SortByYLow {
  bool operator()(const CollisionObject* a, const CollisionObject* b) const {
    if (a->getAABB().min_[1] < b->getAABB().min_[1]) return true;
    return false;
  }
};

/** @brief Functor sorting objects according to the AABB lower z bound */
struct SortByZLow {
  bool operator()(const CollisionObject* a, const CollisionObject* b) const {
    if (a->getAABB().min_[2] < b->getAABB().min_[2]) return true;
    return false;
  }
};

/** @brief Dummy collision object with a point AABB */
class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject {
 public:
  DummyCollisionObject(const AABB& aabb_)
      : CollisionObject(shared_ptr<CollisionGeometry>()) {
    this->aabb = aabb_;
  }

  void computeLocalAABB() {}
};

//==============================================================================
void SSaPCollisionManager::unregisterObject(CollisionObject* obj) {
  setup();

  DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));

  auto pos_start1 = objs_x.begin();
  auto pos_end1 =
      std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());

  while (pos_start1 < pos_end1) {
    if (*pos_start1 == obj) {
      objs_x.erase(pos_start1);
      break;
    }
    ++pos_start1;
  }

  auto pos_start2 = objs_y.begin();
  auto pos_end2 =
      std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());

  while (pos_start2 < pos_end2) {
    if (*pos_start2 == obj) {
      objs_y.erase(pos_start2);
      break;
    }
    ++pos_start2;
  }

  auto pos_start3 = objs_z.begin();
  auto pos_end3 =
      std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());

  while (pos_start3 < pos_end3) {
    if (*pos_start3 == obj) {
      objs_z.erase(pos_start3);
      break;
    }
    ++pos_start3;
  }
}

//==============================================================================
SSaPCollisionManager::SSaPCollisionManager() : setup_(false) {
  // Do nothing
}

//==============================================================================
void SSaPCollisionManager::registerObject(CollisionObject* obj) {
  objs_x.push_back(obj);
  objs_y.push_back(obj);
  objs_z.push_back(obj);
  setup_ = false;
}

//==============================================================================
void SSaPCollisionManager::setup() {
  if (!setup_) {
    std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
    std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
    std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
    setup_ = true;
  }
}

//==============================================================================
void SSaPCollisionManager::update() {
  setup_ = false;
  setup();
}

//==============================================================================
void SSaPCollisionManager::clear() {
  objs_x.clear();
  objs_y.clear();
  objs_z.clear();
  setup_ = false;
}

//==============================================================================
void SSaPCollisionManager::getObjects(
    std::vector<CollisionObject*>& objs) const {
  objs.resize(objs_x.size());
  std::copy(objs_x.begin(), objs_x.end(), objs.begin());
}

//==============================================================================
bool SSaPCollisionManager::checkColl(
    std::vector<CollisionObject*>::const_iterator pos_start,
    std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj,
    CollisionCallBackBase* callback) const {
  while (pos_start < pos_end) {
    if (*pos_start != obj)  // no collision between the same object
    {
      if ((*pos_start)->getAABB().overlap(obj->getAABB())) {
        if ((*callback)(*pos_start, obj)) return true;
      }
    }
    pos_start++;
  }
  return false;
}

//==============================================================================
bool SSaPCollisionManager::checkDis(
    typename std::vector<CollisionObject*>::const_iterator pos_start,
    typename std::vector<CollisionObject*>::const_iterator pos_end,
    CollisionObject* obj, DistanceCallBackBase* callback,
    FCL_REAL& min_dist) const {
  while (pos_start < pos_end) {
    if (*pos_start != obj)  // no distance between the same object
    {
      if ((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) {
        if ((*callback)(*pos_start, obj, min_dist)) return true;
      }
    }
    pos_start++;
  }

  return false;
}

//==============================================================================
void SSaPCollisionManager::collide(CollisionObject* obj,
                                   CollisionCallBackBase* callback) const {
  callback->init();
  if (size() == 0) return;

  collide_(obj, callback);
}

//==============================================================================
bool SSaPCollisionManager::collide_(CollisionObject* obj,
                                    CollisionCallBackBase* callback) const {
  static const unsigned int CUTOFF = 100;

  DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
  bool coll_res = false;

  const auto pos_start1 = objs_x.begin();
  const auto pos_end1 =
      std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
  long d1 = pos_end1 - pos_start1;

  if (d1 > CUTOFF) {
    const auto pos_start2 = objs_y.begin();
    const auto pos_end2 =
        std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
    long d2 = pos_end2 - pos_start2;

    if (d2 > CUTOFF) {
      const auto pos_start3 = objs_z.begin();
      const auto pos_end3 =
          std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
      long d3 = pos_end3 - pos_start3;

      if (d3 > CUTOFF) {
        if (d3 <= d2 && d3 <= d1)
          coll_res = checkColl(pos_start3, pos_end3, obj, callback);
        else {
          if (d2 <= d3 && d2 <= d1)
            coll_res = checkColl(pos_start2, pos_end2, obj, callback);
          else
            coll_res = checkColl(pos_start1, pos_end1, obj, callback);
        }
      } else
        coll_res = checkColl(pos_start3, pos_end3, obj, callback);
    } else
      coll_res = checkColl(pos_start2, pos_end2, obj, callback);
  } else
    coll_res = checkColl(pos_start1, pos_end1, obj, callback);

  return coll_res;
}

//==============================================================================
void SSaPCollisionManager::distance(CollisionObject* obj,
                                    DistanceCallBackBase* callback) const {
  callback->init();
  if (size() == 0) return;

  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
  distance_(obj, callback, min_dist);
}

//==============================================================================
bool SSaPCollisionManager::distance_(CollisionObject* obj,
                                     DistanceCallBackBase* callback,
                                     FCL_REAL& min_dist) const {
  static const unsigned int CUTOFF = 100;
  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
  Vec3f dummy_vector = obj->getAABB().max_;
  if (min_dist < (std::numeric_limits<FCL_REAL>::max)())
    dummy_vector += Vec3f(min_dist, min_dist, min_dist);

  typename std::vector<CollisionObject*>::const_iterator pos_start1 =
      objs_x.begin();
  typename std::vector<CollisionObject*>::const_iterator pos_start2 =
      objs_y.begin();
  typename std::vector<CollisionObject*>::const_iterator pos_start3 =
      objs_z.begin();
  typename std::vector<CollisionObject*>::const_iterator pos_end1 =
      objs_x.end();
  typename std::vector<CollisionObject*>::const_iterator pos_end2 =
      objs_y.end();
  typename std::vector<CollisionObject*>::const_iterator pos_end3 =
      objs_z.end();

  int status = 1;
  FCL_REAL old_min_distance;

  while (1) {
    old_min_distance = min_dist;
    DummyCollisionObject dummyHigh((AABB(dummy_vector)));

    pos_end1 =
        std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
    long d1 = pos_end1 - pos_start1;

    bool dist_res = false;

    if (d1 > CUTOFF) {
      pos_end2 =
          std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
      long d2 = pos_end2 - pos_start2;

      if (d2 > CUTOFF) {
        pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh,
                                    SortByZLow());
        long d3 = pos_end3 - pos_start3;

        if (d3 > CUTOFF) {
          if (d3 <= d2 && d3 <= d1)
            dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
          else {
            if (d2 <= d3 && d2 <= d1)
              dist_res =
                  checkDis(pos_start2, pos_end2, obj, callback, min_dist);
            else
              dist_res =
                  checkDis(pos_start1, pos_end1, obj, callback, min_dist);
          }
        } else
          dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
      } else
        dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist);
    } else {
      dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist);
    }

    if (dist_res) return true;

    if (status == 1) {
      if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
        break;
      else {
        // from infinity to a finite one, only need one additional loop
        // to check the possible missed ones to the right of the objs array
        if (min_dist < old_min_distance) {
          dummy_vector =
              obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
          status = 0;
        } else  // need more loop
        {
          if (dummy_vector.isApprox(
                  obj->getAABB().max_,
                  std::numeric_limits<FCL_REAL>::epsilon() * 100))
            dummy_vector = dummy_vector + delta;
          else
            dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
        }
      }

      // yes, following is wrong, will result in too large distance.
      // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
      // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
      // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
    } else if (status == 0)
      break;
  }

  return false;
}

//==============================================================================
int SSaPCollisionManager::selectOptimalAxis(
    const std::vector<CollisionObject*>& objs_x,
    const std::vector<CollisionObject*>& objs_y,
    const std::vector<CollisionObject*>& objs_z,
    typename std::vector<CollisionObject*>::const_iterator& it_beg,
    typename std::vector<CollisionObject*>::const_iterator& it_end) {
  /// simple sweep and prune method
  FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] -
                     (objs_x[0])->getAABB().min_[0];
  FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] -
                     (objs_y[0])->getAABB().min_[1];
  FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] -
                     (objs_z[0])->getAABB().min_[2];

  int axis = 0;
  if (delta_y > delta_x && delta_y > delta_z)
    axis = 1;
  else if (delta_z > delta_y && delta_z > delta_x)
    axis = 2;

  switch (axis) {
    case 0:
      it_beg = objs_x.begin();
      it_end = objs_x.end();
      break;
    case 1:
      it_beg = objs_y.begin();
      it_end = objs_y.end();
      break;
    case 2:
      it_beg = objs_z.begin();
      it_end = objs_z.end();
      break;
  }

  return axis;
}

//==============================================================================
void SSaPCollisionManager::collide(CollisionCallBackBase* callback) const {
  callback->init();
  if (size() == 0) return;

  typename std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
  int axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end);
  int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
  int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);

  run_pos = pos;

  while ((run_pos < pos_end) && (pos < pos_end)) {
    CollisionObject* obj = *(pos++);

    while (1) {
      if ((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) {
        run_pos++;
        if (run_pos == pos_end) break;
        continue;
      } else {
        run_pos++;
        break;
      }
    }

    if (run_pos < pos_end) {
      typename std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;

      while ((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) {
        CollisionObject* obj2 = *run_pos2;
        run_pos2++;

        if ((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) &&
            (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) {
          if ((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) &&
              (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) {
            if ((*callback)(obj, obj2)) return;
          }
        }

        if (run_pos2 == pos_end) break;
      }
    }
  }
}

//==============================================================================
void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const {
  callback->init();
  if (size() == 0) return;

  typename std::vector<CollisionObject*>::const_iterator it, it_end;
  selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);

  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
  for (; it != it_end; ++it) {
    if (distance_(*it, callback, min_dist)) return;
  }
}

//==============================================================================
void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
                                   CollisionCallBackBase* callback) const {
  callback->init();
  SSaPCollisionManager* other_manager =
      static_cast<SSaPCollisionManager*>(other_manager_);

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

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

  typename std::vector<CollisionObject*>::const_iterator it, end;
  if (this->size() < other_manager->size()) {
    for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
      if (other_manager->collide_(*it, callback)) return;
  } else {
    for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
         it != end; ++it)
      if (collide_(*it, callback)) return;
  }
}

//==============================================================================
void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
                                    DistanceCallBackBase* callback) const {
  callback->init();
  SSaPCollisionManager* other_manager =
      static_cast<SSaPCollisionManager*>(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)();
  typename std::vector<CollisionObject*>::const_iterator it, end;
  if (this->size() < other_manager->size()) {
    for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
      if (other_manager->distance_(*it, callback, min_dist)) return;
  } else {
    for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
         it != end; ++it)
      if (distance_(*it, callback, min_dist)) return;
  }
}

//==============================================================================
bool SSaPCollisionManager::empty() const { return objs_x.empty(); }

//==============================================================================
size_t SSaPCollisionManager::size() const { return objs_x.size(); }

}  // namespace fcl
}  // namespace hpp


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			#include "hpp/fcl/broadphase/broadphase_SSaP.h"
39
40			namespace hpp {
41			namespace fcl {
42
43			/** @brief Functor sorting objects according to the AABB lower x bound */
44			struct SortByXLow {
45		216434	bool operator()(const CollisionObject* a, const CollisionObject* b) const {
46	✓✓	216434	if (a->getAABB().min_[0] < b->getAABB().min_[0]) return true;
47		87915	return false;
48			}
49			};
50
51			/** @brief Functor sorting objects according to the AABB lower y bound */
52			struct SortByYLow {
53		194374	bool operator()(const CollisionObject* a, const CollisionObject* b) const {
54	✓✓	194374	if (a->getAABB().min_[1] < b->getAABB().min_[1]) return true;
55		92493	return false;
56			}
57			};
58
59			/** @brief Functor sorting objects according to the AABB lower z bound */
60			struct SortByZLow {
61		202770	bool operator()(const CollisionObject* a, const CollisionObject* b) const {
62	✓✓	202770	if (a->getAABB().min_[2] < b->getAABB().min_[2]) return true;
63		74901	return false;
64			}
65			};
66
67			/** @brief Dummy collision object with a point AABB */
68			class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject {
69			public:
70		6958	DummyCollisionObject(const AABB& aabb_)
71	✓✗	6958	: CollisionObject(shared_ptr<CollisionGeometry>()) {
72	✓✗	6958	this->aabb = aabb_;
73		6958	}
74
75			void computeLocalAABB() {}
76			};
77
78			//==============================================================================
79			void SSaPCollisionManager::unregisterObject(CollisionObject* obj) {
80			setup();
81
82			DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
83
84			auto pos_start1 = objs_x.begin();
85			auto pos_end1 =
86			std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
87
88			while (pos_start1 < pos_end1) {
89			if (*pos_start1 == obj) {
90			objs_x.erase(pos_start1);
91			break;
92			}
93			++pos_start1;
94			}
95
96			auto pos_start2 = objs_y.begin();
97			auto pos_end2 =
98			std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
99
100			while (pos_start2 < pos_end2) {
101			if (*pos_start2 == obj) {
102			objs_y.erase(pos_start2);
103			break;
104			}
105			++pos_start2;
106			}
107
108			auto pos_start3 = objs_z.begin();
109			auto pos_end3 =
110			std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
111
112			while (pos_start3 < pos_end3) {
113			if (*pos_start3 == obj) {
114			objs_z.erase(pos_start3);
115			break;
116			}
117			++pos_start3;
118			}
119			}
120
121			//==============================================================================
122		51	SSaPCollisionManager::SSaPCollisionManager() : setup_(false) {
123			// Do nothing
124		51	}
125
126			//==============================================================================
127		12671	void SSaPCollisionManager::registerObject(CollisionObject* obj) {
128		12671	objs_x.push_back(obj);
129		12671	objs_y.push_back(obj);
130		12671	objs_z.push_back(obj);
131		12671	setup_ = false;
132		12671	}
133
134			//==============================================================================
135		64	void SSaPCollisionManager::setup() {
136	✓✗	64	if (!setup_) {
137	✓✗	64	std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
138	✓✗	64	std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
139	✓✗	64	std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
140		64	setup_ = true;
141			}
142		64	}
143
144			//==============================================================================
145		13	void SSaPCollisionManager::update() {
146		13	setup_ = false;
147		13	setup();
148		13	}
149
150			//==============================================================================
151			void SSaPCollisionManager::clear() {
152			objs_x.clear();
153			objs_y.clear();
154			objs_z.clear();
155			setup_ = false;
156			}
157
158			//==============================================================================
159			void SSaPCollisionManager::getObjects(
160			std::vector<CollisionObject*>& objs) const {
161			objs.resize(objs_x.size());
162			std::copy(objs_x.begin(), objs_x.end(), objs.begin());
163			}
164
165			//==============================================================================
166		178668	bool SSaPCollisionManager::checkColl(
167			std::vector<CollisionObject*>::const_iterator pos_start,
168			std::vector<CollisionObject>::const_iterator pos_end, CollisionObject obj,
169			CollisionCallBackBase* callback) const {
170	✓✓	178668	while (pos_start < pos_end) {
171	✓✗	174906	if (*pos_start != obj) // no collision between the same object
172			{
173	✓✓	174906	if ((*pos_start)->getAABB().overlap(obj->getAABB())) {
174	✗✓	10	if ((callback)(pos_start, obj)) return true;
175			}
176			}
177		174906	pos_start++;
178			}
179		3762	return false;
180			}
181
182			//==============================================================================
183		924814	bool SSaPCollisionManager::checkDis(
184			typename std::vector<CollisionObject*>::const_iterator pos_start,
185			typename std::vector<CollisionObject*>::const_iterator pos_end,
186			CollisionObject* obj, DistanceCallBackBase* callback,
187			FCL_REAL& min_dist) const {
188	✓✓	924814	while (pos_start < pos_end) {
189	✓✓	921618	if (*pos_start != obj) // no distance between the same object
190			{
191	✓✓	918582	if ((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) {
192	✗✓	3072	if ((callback)(pos_start, obj, min_dist)) return true;
193			}
194			}
195		921618	pos_start++;
196			}
197
198		3196	return false;
199			}
200
201			//==============================================================================
202		3822	void SSaPCollisionManager::collide(CollisionObject* obj,
203			CollisionCallBackBase* callback) const {
204		3822	callback->init();
205	✓✓	3822	if (size() == 0) return;
206
207		3762	collide_(obj, callback);
208			}
209
210			//==============================================================================
211		3762	bool SSaPCollisionManager::collide_(CollisionObject* obj,
212			CollisionCallBackBase* callback) const {
213			static const unsigned int CUTOFF = 100;
214
215	✓✗✓✗	3762	DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
216		3762	bool coll_res = false;
217
218		3762	const auto pos_start1 = objs_x.begin();
219			const auto pos_end1 =
220	✓✗	3762	std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
221		3762	long d1 = pos_end1 - pos_start1;
222
223	✓✓	3762	if (d1 > CUTOFF) {
224		1209	const auto pos_start2 = objs_y.begin();
225			const auto pos_end2 =
226	✓✗	1209	std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
227		1209	long d2 = pos_end2 - pos_start2;
228
229	✓✓	1209	if (d2 > CUTOFF) {
230		812	const auto pos_start3 = objs_z.begin();
231			const auto pos_end3 =
232	✓✗	812	std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
233		812	long d3 = pos_end3 - pos_start3;
234
235	✓✓	812	if (d3 > CUTOFF) {
236	✓✓✓✓	552	if (d3 <= d2 && d3 <= d1)
237	✓✗	204	coll_res = checkColl(pos_start3, pos_end3, obj, callback);
238			else {
239	✓✓✓✓	348	if (d2 <= d3 && d2 <= d1)
240	✓✗	191	coll_res = checkColl(pos_start2, pos_end2, obj, callback);
241			else
242	✓✗	157	coll_res = checkColl(pos_start1, pos_end1, obj, callback);
243			}
244			} else
245	✓✗	260	coll_res = checkColl(pos_start3, pos_end3, obj, callback);
246			} else
247	✓✗	397	coll_res = checkColl(pos_start2, pos_end2, obj, callback);
248			} else
249	✓✗	2553	coll_res = checkColl(pos_start1, pos_end1, obj, callback);
250
251		7524	return coll_res;
252			}
253
254			//==============================================================================
255		80	void SSaPCollisionManager::distance(CollisionObject* obj,
256			DistanceCallBackBase* callback) const {
257	✓✗	80	callback->init();
258	✓✗✗✓	80	if (size() == 0) return;
259
260		80	FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
261	✓✗	80	distance_(obj, callback, min_dist);
262			}
263
264			//==============================================================================
265		3070	bool SSaPCollisionManager::distance_(CollisionObject* obj,
266			DistanceCallBackBase* callback,
267			FCL_REAL& min_dist) const {
268			static const unsigned int CUTOFF = 100;
269	✓✗✓✗ ✓✗	3070	Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
270	✓✗	3070	Vec3f dummy_vector = obj->getAABB().max_;
271	✓✓	3070	if (min_dist < (std::numeric_limits<FCL_REAL>::max)())
272	✓✗✓✗	2984	dummy_vector += Vec3f(min_dist, min_dist, min_dist);
273
274			typename std::vector<CollisionObject*>::const_iterator pos_start1 =
275		3070	objs_x.begin();
276			typename std::vector<CollisionObject*>::const_iterator pos_start2 =
277		3070	objs_y.begin();
278			typename std::vector<CollisionObject*>::const_iterator pos_start3 =
279		3070	objs_z.begin();
280			typename std::vector<CollisionObject*>::const_iterator pos_end1 =
281		3070	objs_x.end();
282			typename std::vector<CollisionObject*>::const_iterator pos_end2 =
283		3070	objs_y.end();
284			typename std::vector<CollisionObject*>::const_iterator pos_end3 =
285		3070	objs_z.end();
286
287		3070	int status = 1;
288			FCL_REAL old_min_distance;
289
290			while (1) {
291		3196	old_min_distance = min_dist;
292	✓✗✓✗	3196	DummyCollisionObject dummyHigh((AABB(dummy_vector)));
293
294			pos_end1 =
295	✓✗	3196	std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
296		3196	long d1 = pos_end1 - pos_start1;
297
298		3196	bool dist_res = false;
299
300	✓✓	3196	if (d1 > CUTOFF) {
301			pos_end2 =
302	✓✗	2757	std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
303		2757	long d2 = pos_end2 - pos_start2;
304
305	✓✓	2757	if (d2 > CUTOFF) {
306			pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh,
307	✓✗	2739	SortByZLow());
308		2739	long d3 = pos_end3 - pos_start3;
309
310	✓✓	2739	if (d3 > CUTOFF) {
311	✓✓✓✓	2289	if (d3 <= d2 && d3 <= d1)
312	✓✗	1134	dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
313			else {
314	✓✓✓✓	1155	if (d2 <= d3 && d2 <= d1)
315		20	dist_res =
316	✓✗	20	checkDis(pos_start2, pos_end2, obj, callback, min_dist);
317			else
318			dist_res =
319	✓✗	1135	checkDis(pos_start1, pos_end1, obj, callback, min_dist);
320			}
321			} else
322	✓✗	450	dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
323			} else
324	✓✗	18	dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist);
325			} else {
326	✓✗	439	dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist);
327			}
328
329	✗✓	3196	if (dist_res) return true;
330
331	✓✓	3196	if (status == 1) {
332	✓✓	3110	if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
333		2984	break;
334			else {
335			// from infinity to a finite one, only need one additional loop
336			// to check the possible missed ones to the right of the objs array
337	✓✓	126	if (min_dist < old_min_distance) {
338			dummy_vector =
339	✓✗✓✗ ✓✗	86	obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
340		86	status = 0;
341			} else // need more loop
342			{
343		80	if (dummy_vector.isApprox(
344	✓✗	40	obj->getAABB().max_,
345	✓✓	80	std::numeric_limits<FCL_REAL>::epsilon() * 100))
346	✓✗✓✗	4	dummy_vector = dummy_vector + delta;
347			else
348	✓✗✓✗ ✓✗	36	dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
349			}
350			}
351
352			// yes, following is wrong, will result in too large distance.
353			// if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
354			// if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
355			// if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
356	✓✗	86	} else if (status == 0)
357		86	break;
358		126	}
359
360		3070	return false;
361			}
362
363			//==============================================================================
364		35	int SSaPCollisionManager::selectOptimalAxis(
365			const std::vector<CollisionObject*>& objs_x,
366			const std::vector<CollisionObject*>& objs_y,
367			const std::vector<CollisionObject*>& objs_z,
368			typename std::vector<CollisionObject*>::const_iterator& it_beg,
369			typename std::vector<CollisionObject*>::const_iterator& it_end) {
370			/// simple sweep and prune method
371		35	FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] -
372		35	(objs_x[0])->getAABB().min_[0];
373		35	FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] -
374		35	(objs_y[0])->getAABB().min_[1];
375		35	FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] -
376		35	(objs_z[0])->getAABB().min_[2];
377
378		35	int axis = 0;
379	✓✓✓✓	35	if (delta_y > delta_x && delta_y > delta_z)
380		2	axis = 1;
381	✓✓✓✓	33	else if (delta_z > delta_y && delta_z > delta_x)
382		21	axis = 2;
383
384	✓✓✓✗	35	switch (axis) {
385		12	case 0:
386		12	it_beg = objs_x.begin();
387		12	it_end = objs_x.end();
388		12	break;
389		2	case 1:
390		2	it_beg = objs_y.begin();
391		2	it_end = objs_y.end();
392		2	break;
393		21	case 2:
394		21	it_beg = objs_z.begin();
395		21	it_end = objs_z.end();
396		21	break;
397			}
398
399		35	return axis;
400			}
401
402			//==============================================================================
403		37	void SSaPCollisionManager::collide(CollisionCallBackBase* callback) const {
404	✓✗	37	callback->init();
405	✓✗✓✓	38	if (size() == 0) return;
406
407		29	typename std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
408	✓✗	29	int axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end);
409	✓✓	29	int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
410	✓✓	29	int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
411
412		29	run_pos = pos;
413
414	✓✓✓✗ ✓✓	2363	while ((run_pos < pos_end) && (pos < pos_end)) {
415		2335	CollisionObject* obj = *(pos++);
416
417			while (1) {
418	✓✗✓✗ ✗✓	2335	if ((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) {
419			run_pos++;
420			if (run_pos == pos_end) break;
421			continue;
422			} else {
423		2335	run_pos++;
424		2335	break;
425			}
426			}
427
428	✓✓	2335	if (run_pos < pos_end) {
429		2307	typename std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
430
431	✓✗✓✗ ✓✓	8717	while ((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) {
432		6426	CollisionObject* obj2 = *run_pos2;
433		6426	run_pos2++;
434
435	✓✗✓✗ ✓✓✓✓	9725	if ((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) &&
436	✓✗✓✗ ✓✓	3299	(obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) {
437	✓✗✓✗ ✓✓✓✓	232	if ((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) &&
438	✓✗✓✗ ✓✓	79	(obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) {
439	✓✗✓✓	6	if ((*callback)(obj, obj2)) return;
440			}
441			}
442
443	✓✓	6425	if (run_pos2 == pos_end) break;
444			}
445			}
446			}
447			}
448
449			//==============================================================================
450		6	void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const {
451	✓✗	6	callback->init();
452	✓✗✗✓	6	if (size() == 0) return;
453
454		6	typename std::vector<CollisionObject*>::const_iterator it, it_end;
455	✓✗	6	selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
456
457		6	FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
458	✓✓	2996	for (; it != it_end; ++it) {
459	✓✗✗✓	2990	if (distance_(*it, callback, min_dist)) return;
460			}
461			}
462
463			//==============================================================================
464			void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
465			CollisionCallBackBase* callback) const {
466			callback->init();
467			SSaPCollisionManager* other_manager =
468			static_cast<SSaPCollisionManager*>(other_manager_);
469
470			if ((size() == 0) \|\| (other_manager->size() == 0)) return;
471
472			if (this == other_manager) {
473			collide(callback);
474			return;
475			}
476
477			typename std::vector<CollisionObject*>::const_iterator it, end;
478			if (this->size() < other_manager->size()) {
479			for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
480			if (other_manager->collide_(*it, callback)) return;
481			} else {
482			for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
483			it != end; ++it)
484			if (collide_(*it, callback)) return;
485			}
486			}
487
488			//==============================================================================
489			void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
490			DistanceCallBackBase* callback) const {
491			callback->init();
492			SSaPCollisionManager* other_manager =
493			static_cast<SSaPCollisionManager*>(other_manager_);
494
495			if ((size() == 0) \|\| (other_manager->size() == 0)) return;
496
497			if (this == other_manager) {
498			distance(callback);
499			return;
500			}
501
502			FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
503			typename std::vector<CollisionObject*>::const_iterator it, end;
504			if (this->size() < other_manager->size()) {
505			for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
506			if (other_manager->distance_(*it, callback, min_dist)) return;
507			} else {
508			for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
509			it != end; ++it)
510			if (distance_(*it, callback, min_dist)) return;
511			}
512			}
513
514			//==============================================================================
515			bool SSaPCollisionManager::empty() const { return objs_x.empty(); }
516
517			//==============================================================================
518		3945	size_t SSaPCollisionManager::size() const { return objs_x.size(); }
519
520			} // namespace fcl
521			} // namespace hpp