Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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
- {
Add Comment
Please, Sign In to add comment