From 716dcb8f35940510b38b8da0fe6ca17eace95d5d Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Thu, 8 Sep 2011 03:13:43 +0000
Subject: [PATCH] fix the bug which causes runtime error in grasping
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@26 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/src/broad_phase_collision.cpp | 7 +++++--
1 file changed, 5 insertions(+), 2 deletions(-)
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index f600d4ee..ca3b2f8f 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -41,6 +41,8 @@
#include <set>
#include <deque>
+#include <iostream>
+
namespace fcl
{
@@ -139,7 +141,6 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
DummyCollisionObject dummyHigh(AABB(obj->getCachedAABB().max_));
- CollisionObject* found = NULL;
std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
@@ -147,7 +148,6 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
{
if(*pos_start1 == obj)
{
- found = *pos_start1;
objs_x.erase(pos_start1);
break;
}
@@ -156,6 +156,7 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
std::vector<CollisionObject*>::iterator pos_start2 = objs_y.begin();
std::vector<CollisionObject*>::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
+
while(pos_start2 < pos_end2)
{
if(*pos_start2 == obj)
@@ -168,6 +169,7 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
std::vector<CollisionObject*>::iterator pos_start3 = objs_z.begin();
std::vector<CollisionObject*>::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
+
while(pos_start3 < pos_end3)
{
if(*pos_start3 == obj)
@@ -201,6 +203,7 @@ void SSaPCollisionManager::setup()
void SSaPCollisionManager::update()
{
+ setup_ = false;
setup();
}
--
GitLab