SHARE
TWEET

Untitled

a guest Nov 14th, 2017 60 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. diff --git a/src/rl/plan/CMakeLists.txt b/src/rl/plan/CMakeLists.txt
  2. index 3a62575..0953afe 100644
  3. --- a/src/rl/plan/CMakeLists.txt
  4. +++ b/src/rl/plan/CMakeLists.txt
  5. @@ -2,7 +2,7 @@ project(rlplan)
  6.  
  7.  find_package(Boost COMPONENTS thread system REQUIRED)
  8.  
  9. -find_package(Cgal REQUIRED)
  10. +find_package(CGAL REQUIRED)
  11.  
  12.  set(
  13.     HDRS
  14. diff --git a/src/rl/sg/bullet/Scene.cpp b/src/rl/sg/bullet/Scene.cpp
  15. index 5cdc2e3..3f336ad 100644
  16. --- a/src/rl/sg/bullet/Scene.cpp
  17. +++ b/src/rl/sg/bullet/Scene.cpp
  18. @@ -35,6 +35,7 @@
  19.  #include "Model.h"
  20.  #include "Scene.h"
  21.  #include "Shape.h"
  22. +#include <iostream>
  23.  
  24.  namespace rl
  25.  {
  26. @@ -70,9 +71,24 @@ namespace rl
  27.                 Body* body2 = static_cast< Body* >(second);
  28.                
  29.                 ContactResultCallback resultCallback;
  30. +                                resultCallback.clearDatas();
  31.                 this->world.contactPairTest(&body1->object, &body2->object, resultCallback);
  32. -              
  33. -               return resultCallback.collision;
  34. +
  35. +//                                std::cout << "resultCallback.collision:" << resultCallback.collision << std::endl;
  36. +
  37. +//                                return resultCallback.collision;
  38. +                                double maxDistance = 0.0;
  39. +                                for(size_t i = 0; i< resultCallback.collisionVec.size(); ++i)
  40. +                                {
  41. +//                                    std::cout << "distance: " << resultCallback.distanceVec[i] << std::endl;
  42. +                                    if( resultCallback.distanceVec[i] < maxDistance)
  43. +                                    { maxDistance = resultCallback.distanceVec[i]; }
  44. +                                }
  45. +//                                std::cout << "maxDistance: " << maxDistance << std::endl;
  46. +                                if( (resultCallback.collisionVec.size() > 0) && (maxDistance < 0.0) )
  47. +                                    return true;
  48. +                                else
  49. +                                    return false;
  50.             }
  51.            
  52.             bool
  53. @@ -294,26 +310,55 @@ namespace rl
  54.                 }
  55.             }
  56.            
  57. -           Scene::ContactResultCallback::ContactResultCallback() :
  58. -               collision(false)
  59. -           {
  60. -           }
  61. +                        Scene::ContactResultCallback::ContactResultCallback():
  62. +                            collision(false),
  63. +                            distance(0),
  64. +                            positionWorldOnA(0.0, 0.0, 0.0),
  65. +                            positionWorldOnB(0.0, 0.0, 0.0),
  66. +                            localPointA(0.0, 0.0, 0.0),
  67. +                            localPointB(0.0, 0.0, 0.0),
  68. +                            normalWorldOnB(0.0, 0.0, 0.0)
  69. +                        {
  70. +                        }
  71. +
  72. +                        void Scene::ContactResultCallback::clearDatas()
  73. +                        {
  74. +                            this->collision = false;
  75. +                            this->distance = 0;
  76. +                            this->positionWorldOnA.setValue(0.0, 0.0, 0.0);
  77. +                            this->positionWorldOnB.setValue(0.0, 0.0, 0.0);
  78. +                            this->normalWorldOnB.setValue(0.0, 0.0, 0.0);
  79. +                            this->localPointA.setValue(0.0, 0.0, 0.0);
  80. +                            this->localPointB.setValue(0.0, 0.0, 0.0);
  81. +
  82. +                            this->collisionVec.clear();
  83. +                            this->distanceVec.clear();
  84. +                            this->posWorldOnAVec.clear();
  85. +                            this->posWorldOnBVec.clear();
  86. +                            this->normalWorldOnBVec.clear();
  87. +                        }
  88. +
  89.            
  90. -           btScalar
  91. -#if (BT_BULLET_VERSION < 281)
  92. -           Scene::ContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1)
  93. -#else
  94. -           Scene::ContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1)
  95. -#endif
  96. -           {
  97. -               this->collision = true;
  98. -#if 0
  99. -               distance = cp.getDistance();
  100. -               positionWorldOnA = cp.getPositionWorldOnA();
  101. -               positionWorldOnB = cp.getPositionWorldOnB();
  102. -#endif
  103. -               return 0;
  104. -           }
  105. +
  106. +                        btScalar Scene::ContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1)
  107. +                        {
  108. +                            this->collision = true;
  109. +                            this->distance = cp.getDistance();
  110. +                            this->positionWorldOnA = cp.getPositionWorldOnA();
  111. +                            this->positionWorldOnB = cp.getPositionWorldOnB();
  112. +                            this->normalWorldOnB = cp.m_normalWorldOnB;
  113. +                            this->localPointA = cp.m_localPointA;
  114. +                            this->localPointB = cp.m_localPointB;
  115. +
  116. +                            this->collisionVec.push_back(collision);
  117. +                            this->distanceVec.push_back(distance);
  118. +                            this->posWorldOnAVec.push_back(positionWorldOnA);
  119. +                            this->posWorldOnBVec.push_back(positionWorldOnB);
  120. +                            this->normalWorldOnBVec.push_back(normalWorldOnB);
  121. +
  122. +                            return 0;
  123. +                        }
  124. +
  125.            
  126.             Scene::RayResultCallback::RayResultCallback() :
  127.                 collisionShape(NULL)
  128. diff --git a/src/rl/sg/bullet/Scene.h b/src/rl/sg/bullet/Scene.h
  129. index c5ef3a2..ad7983e 100644
  130. --- a/src/rl/sg/bullet/Scene.h
  131. +++ b/src/rl/sg/bullet/Scene.h
  132. @@ -83,24 +83,29 @@ namespace rl
  133.             protected:
  134.                
  135.             private:
  136. -               struct ContactResultCallback : public btCollisionWorld::ContactResultCallback
  137. -               {
  138. -                   ContactResultCallback();
  139. -                  
  140. -#if (BT_BULLET_VERSION < 281)
  141. -                   btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1);
  142. -#else
  143. -                   btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1);
  144. -#endif
  145. -                  
  146. -                   bool collision;
  147. -                  
  148. -                   btScalar distance;
  149. -                  
  150. -                   btVector3 positionWorldOnA;
  151. -                  
  152. -                   btVector3 positionWorldOnB;
  153. -               };
  154. +                                struct ContactResultCallback : public btCollisionWorld::ContactResultCallback
  155. +                                {
  156. +                                    ContactResultCallback();
  157. +
  158. +                                    btScalar addSingleResult(btManifoldPoint& cp, const ::btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1);  //define your code here!
  159. +
  160. +                                    void clearDatas();
  161. +
  162. +                                    bool collision;
  163. +                                    btScalar distance;
  164. +                                    btVector3 positionWorldOnA;
  165. +                                    btVector3 positionWorldOnB;
  166. +
  167. +                                    btVector3 localPointA;
  168. +                                    btVector3 localPointB;
  169. +                                    btVector3 normalWorldOnB;
  170. +
  171. +                                    std::vector<bool> collisionVec;
  172. +                                    std::vector<btScalar> distanceVec;
  173. +                                    std::vector<btVector3> posWorldOnAVec;
  174. +                                    std::vector<btVector3> posWorldOnBVec;
  175. +                                    std::vector<btVector3> normalWorldOnBVec;
  176. +                                };
  177.                
  178.                 struct RayResultCallback : public btCollisionWorld::RayResultCallback
  179.                 {
RAW Paste Data
Top