rl-patch-callback-collision-dist.diff

rl-patch-callback-collision-dist.diff

diff --git a/src/rl/hal/CMakeLists.txt b/src/rl/hal/CMakeLists.txt
index a21695b..d7704d3 100644
--- a/src/rl/hal/CMakeLists.txt
+++ b/src/rl/hal/CMakeLists.txt
@@ -2,7 +2,7 @@ project(rlhal)
 
 find_package(Atidaq)
 find_package(Comedi)
-find_package(Libdc1394)
+#find_package(Libdc1394)
 
 include(TestBigEndian)
 
diff --git a/src/rl/plan/CMakeLists.txt b/src/rl/plan/CMakeLists.txt
index 3a62575..0953afe 100644
--- a/src/rl/plan/CMakeLists.txt
+++ b/src/rl/plan/CMakeLists.txt
@@ -2,7 +2,7 @@ project(rlplan)
 
 find_package(Boost COMPONENTS thread system REQUIRED)
 
-find_package(Cgal REQUIRED)
+find_package(CGAL REQUIRED)
 
 set(
 	HDRS
diff --git a/src/rl/sg/bullet/Scene.cpp b/src/rl/sg/bullet/Scene.cpp
index 5cdc2e3..3f336ad 100644
--- a/src/rl/sg/bullet/Scene.cpp
+++ b/src/rl/sg/bullet/Scene.cpp
@@ -35,6 +35,7 @@
 #include "Model.h"
 #include "Scene.h"
 #include "Shape.h"
+#include <iostream>
 
 namespace rl
 {
@@ -70,9 +71,24 @@ namespace rl
 				Body* body2 = static_cast< Body* >(second);
 				
 				ContactResultCallback resultCallback;
+                                resultCallback.clearDatas();
 				this->world.contactPairTest(&body1->object, &body2->object, resultCallback);
-				
-				return resultCallback.collision;
+
+//                                std::cout << "resultCallback.collision:" << resultCallback.collision << std::endl;
+				
+//                                return resultCallback.collision;
+                                double maxDistance = 0.0;
+                                for(size_t i = 0; i< resultCallback.collisionVec.size(); ++i)
+                                {
+//                                    std::cout << "distance: " << resultCallback.distanceVec[i] << std::endl;
+                                    if( resultCallback.distanceVec[i] < maxDistance)
+                                    { maxDistance = resultCallback.distanceVec[i]; }
+                                }
+//                                std::cout << "maxDistance: " << maxDistance << std::endl;
+                                if( (resultCallback.collisionVec.size() > 0) && (maxDistance < 0.0) )
+                                    return true;
+                                else
+                                    return false;
 			}
 			
 			bool
@@ -294,26 +310,55 @@ namespace rl
 				}
 			}
 			
-			Scene::ContactResultCallback::ContactResultCallback() :
-				collision(false)
-			{
-			}
+                        Scene::ContactResultCallback::ContactResultCallback():
+                            collision(false),
+                            distance(0),
+                            positionWorldOnA(0.0, 0.0, 0.0),
+                            positionWorldOnB(0.0, 0.0, 0.0),
+                            localPointA(0.0, 0.0, 0.0),
+                            localPointB(0.0, 0.0, 0.0),
+                            normalWorldOnB(0.0, 0.0, 0.0)
+                        {
+                        }
+
+                        void Scene::ContactResultCallback::clearDatas()
+                        {
+                            this->collision = false;
+                            this->distance = 0;
+                            this->positionWorldOnA.setValue(0.0, 0.0, 0.0);
+                            this->positionWorldOnB.setValue(0.0, 0.0, 0.0);
+                            this->normalWorldOnB.setValue(0.0, 0.0, 0.0);
+                            this->localPointA.setValue(0.0, 0.0, 0.0);
+                            this->localPointB.setValue(0.0, 0.0, 0.0);
+
+                            this->collisionVec.clear();
+                            this->distanceVec.clear();
+                            this->posWorldOnAVec.clear();
+                            this->posWorldOnBVec.clear();
+                            this->normalWorldOnBVec.clear();
+                        }
+
 			
-			btScalar
-#if (BT_BULLET_VERSION < 281)
-			Scene::ContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1)
-#else
-			Scene::ContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1)
-#endif
-			{
-				this->collision = true;
-#if 0
-				distance = cp.getDistance();
-				positionWorldOnA = cp.getPositionWorldOnA();
-				positionWorldOnB = cp.getPositionWorldOnB();
-#endif
-				return 0;
-			}
+
+                        btScalar Scene::ContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1)
+                        {
+                            this->collision = true;
+                            this->distance = cp.getDistance();
+                            this->positionWorldOnA = cp.getPositionWorldOnA();
+                            this->positionWorldOnB = cp.getPositionWorldOnB();
+                            this->normalWorldOnB = cp.m_normalWorldOnB;
+                            this->localPointA = cp.m_localPointA;
+                            this->localPointB = cp.m_localPointB;
+
+                            this->collisionVec.push_back(collision);
+                            this->distanceVec.push_back(distance);
+                            this->posWorldOnAVec.push_back(positionWorldOnA);
+                            this->posWorldOnBVec.push_back(positionWorldOnB);
+                            this->normalWorldOnBVec.push_back(normalWorldOnB);
+
+                            return 0;
+                        }
+
 			
 			Scene::RayResultCallback::RayResultCallback() :
 				collisionShape(NULL)
diff --git a/src/rl/sg/bullet/Scene.h b/src/rl/sg/bullet/Scene.h
index c5ef3a2..ad7983e 100644
--- a/src/rl/sg/bullet/Scene.h
+++ b/src/rl/sg/bullet/Scene.h
@@ -83,24 +83,29 @@ namespace rl
 			protected:
 				
 			private:
-				struct ContactResultCallback : public btCollisionWorld::ContactResultCallback 
-				{
-					ContactResultCallback();
-					
-#if (BT_BULLET_VERSION < 281)
-					btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1);
-#else
-					btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1);
-#endif
-					
-					bool collision;
-					
-					btScalar distance;
-					
-					btVector3 positionWorldOnA;
-					
-					btVector3 positionWorldOnB;
-				};
+                                struct ContactResultCallback : public btCollisionWorld::ContactResultCallback
+                                {
+                                    ContactResultCallback();
+
+                                    btScalar addSingleResult(btManifoldPoint& cp, const ::btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1);  //define your code here!
+
+                                    void clearDatas();
+
+                                    bool collision;
+                                    btScalar distance;
+                                    btVector3 positionWorldOnA;
+                                    btVector3 positionWorldOnB;
+
+                                    btVector3 localPointA;
+                                    btVector3 localPointB;
+                                    btVector3 normalWorldOnB;
+
+                                    std::vector<bool> collisionVec;
+                                    std::vector<btScalar> distanceVec;
+                                    std::vector<btVector3> posWorldOnAVec;
+                                    std::vector<btVector3> posWorldOnBVec;
+                                    std::vector<btVector3> normalWorldOnBVec;
+                                };
 				
 				struct RayResultCallback : public btCollisionWorld::RayResultCallback
 				{