Guest User

Untitled

a guest
Nov 14th, 2017
111
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 7.65 KB | None | 0 0
  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. {
Add Comment
Please, Sign In to add comment