From 37d59b8633ce6f4dbee0ed29a3875015a277cb0c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 15 Feb 2016 09:42:08 -0500 Subject: [PATCH 01/67] Decoupling collision detection engine and collision objects from CollisionDetector --- dart/collision/CollisionDetector.h | 6 + dart/collision/CollisionObject.cpp | 125 ++++ dart/collision/CollisionObject.h | 139 ++++ dart/collision/CollisionObjectData.cpp | 45 ++ dart/collision/CollisionObjectData.h | 60 ++ dart/collision/Engine.cpp | 130 ++++ dart/collision/Engine.h | 103 +++ dart/collision/fcl/FCLCollisionDetector.cpp | 2 +- dart/collision/fcl/FCLCollisionNode.cpp | 240 ++++++- dart/collision/fcl/FCLCollisionNode.h | 41 +- dart/collision/fcl/FCLCollisionObjectData.cpp | 622 ++++++++++++++++++ dart/collision/fcl/FCLCollisionObjectData.h | 92 +++ dart/collision/fcl/FCLEngine.cpp | 239 +++++++ dart/collision/fcl/FCLEngine.h | 64 ++ dart/collision/fcl/FCLTypes.h | 9 + unittests/testCollision.cpp | 34 + unittests/testConstraint.cpp | 2 +- 17 files changed, 1910 insertions(+), 43 deletions(-) create mode 100644 dart/collision/CollisionObject.cpp create mode 100644 dart/collision/CollisionObject.h create mode 100644 dart/collision/CollisionObjectData.cpp create mode 100644 dart/collision/CollisionObjectData.h create mode 100644 dart/collision/Engine.cpp create mode 100644 dart/collision/Engine.h create mode 100644 dart/collision/fcl/FCLCollisionObjectData.cpp create mode 100644 dart/collision/fcl/FCLCollisionObjectData.h create mode 100644 dart/collision/fcl/FCLEngine.cpp create mode 100644 dart/collision/fcl/FCLEngine.h diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 25b4d77804421..0809e63a017dc 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -49,6 +49,8 @@ namespace dart { namespace collision { +class CollisionObject; + /// Contact information struct Contact { // To get byte-aligned Eigen vectors @@ -78,6 +80,10 @@ struct Contact { /// Second colliding shape of the first body node dynamics::ShapePtr shape2; + collision::CollisionObject* collisionObject1; + collision::CollisionObject* collisionObject2; + // TODO(JS): shared_ptr + /// Penetration depth double penetrationDepth; diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp new file mode 100644 index 0000000000000..5b0ad6703b787 --- /dev/null +++ b/dart/collision/CollisionObject.cpp @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/CollisionObject.h" + +#include "dart/collision/CollisionObjectData.h" + +namespace dart { +namespace collision { + +//============================================================================== +dynamics::ShapePtr CollisionObject::getShape() const +{ + return mShape; +} + +//============================================================================== +//void CollisionObject::reportCollision() +//{ +// // Do nothing +//} + +//============================================================================== +EngineType CollisionObject::getEngineType() const +{ + return mEngineType; +} + +//============================================================================== +CollisionObjectData* CollisionObject::getEngineData() const +{ + return mEngineData.get(); +} + +//============================================================================== +void CollisionObject::updateEngineData() +{ + mEngineData->updateTransform(getTransform()); +} + +//============================================================================== +bool CollisionObject::detect(CollisionObject* other, + const Option& option, + Result& result) +{ + return Engine::detect(this, other, option, result); +} + +//============================================================================== +CollisionObject::CollisionObject(const dynamics::ShapePtr& shape, + EngineType type) + : mShape(shape), + mEngineType(type) +{ + mEngineData.reset( + Engine::createCollisionObjectData(mEngineType, this, mShape)); +} + +//============================================================================== +FreeCollisionObject::FreeCollisionObject(const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf, EngineType type) + : CollisionObject(shape, type), + mW(Eigen::Isometry3d::Identity()) +{ + // Do nothing +} + +//============================================================================== +void FreeCollisionObject::setTransform(const Eigen::Isometry3d& tf) +{ + mW = tf; +} + +//============================================================================== +void FreeCollisionObject::setRotation(const Eigen::Matrix3d& rotation) +{ + mW.linear() = rotation; +} + +//============================================================================== +void FreeCollisionObject::setTranslation(const Eigen::Vector3d& translation) +{ + mW.translation() = translation; +} + +//============================================================================== +const Eigen::Isometry3d FreeCollisionObject::getTransform() const +{ + return mW; +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h new file mode 100644 index 0000000000000..1428cf6fa0c29 --- /dev/null +++ b/dart/collision/CollisionObject.h @@ -0,0 +1,139 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_COLLISIONOBJECT_H_ +#define DART_COLLISION_COLLISIONOBJECT_H_ + +#include +#include + +#include "dart/collision/Contact.h" +#include "dart/collision/Engine.h" +#include "dart/collision/CollisionObjectData.h" + +namespace dart { +namespace collision { + +class CollisionObjectData; + +class CollisionObject +{ +public: + + virtual const Eigen::Isometry3d getTransform() const = 0; + + /// Return shape pointer that associated with this CollisionObject + dynamics::ShapePtr getShape() const; + // TODO(JS): Shape should be in common or math + + // virtual void reportCollision(); + + EngineType getEngineType() const; + + // virtual void changeEngine(EngineType type) {} + + CollisionObjectData* getEngineData() const; + + /// Update engine data. This function should be called before the collision + /// detection is performed by the engine in most cases. + void updateEngineData(); + + /// Perform collision detection with other CollisionObject. Return false if + /// the engine type of the other CollisionObject is different from this + /// CollisionObject's. + bool detect(CollisionObject* other, + const collision::Option& option, + collision::Result& result); + + // bool detect(CollisionGroup* group, + // const collision::Option& option, + // collision::Result& result); + +protected: + + /// Contructor + CollisionObject(const dynamics::ShapePtr& shape, EngineType type); + +protected: + + /// Shape + dynamics::ShapePtr mShape; + + /// Collision engine type + EngineType mEngineType; + + /// Engine specific data + std::shared_ptr mEngineData; + +}; + +/// FreeCollisionOjbect is a basic collision object whose transformation can be +/// set freely. +class FreeCollisionObject : public CollisionObject +{ +public: + + /// Constructor + FreeCollisionObject( + const dynamics::ShapePtr& shape, + const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), + EngineType type = FCL); + + /// Set world transformation of this FreeCollisionObject + void setTransform(const Eigen::Isometry3d& tf); + + /// Set world rotation of this FreeCollisionObject + void setRotation(const Eigen::Matrix3d& rotation); + + /// Set world translation of this FreeCollisionObject + void setTranslation(const Eigen::Vector3d& translation); + + /// Return world transformation of this FreeCollisionObject + const Eigen::Isometry3d getTransform() const override; + +protected: + + /// Transformation in world coordinates + Eigen::Isometry3d mW; + +}; + +using CollisionObjectPtr = std::shared_ptr; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONOBJECT_H_ diff --git a/dart/collision/CollisionObjectData.cpp b/dart/collision/CollisionObjectData.cpp new file mode 100644 index 0000000000000..c05a6161e4903 --- /dev/null +++ b/dart/collision/CollisionObjectData.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/CollisionObjectData.h" + +#include "dart/dynamics/BodyNode.h" + +namespace dart { +namespace collision { + +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionObjectData.h b/dart/collision/CollisionObjectData.h new file mode 100644 index 0000000000000..1f85abc52c04e --- /dev/null +++ b/dart/collision/CollisionObjectData.h @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_COLLISIONOBJECTDATA_H_ +#define DART_COLLISION_COLLISIONOBJECTDATA_H_ + +#include + +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace collision { + +class CollisionObjectData +{ +public: + + virtual void updateTransform(const Eigen::Isometry3d& tf) = 0; + + virtual void updateShape(const dynamics::ShapePtr& shape) = 0; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONOBJECTDATA_H_ diff --git a/dart/collision/Engine.cpp b/dart/collision/Engine.cpp new file mode 100644 index 0000000000000..3ce87ddf65a13 --- /dev/null +++ b/dart/collision/Engine.cpp @@ -0,0 +1,130 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/Engine.h" + +#include +#include +#include + +#include "dart/common/Console.h" +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/collision/CollisionNode.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/fcl/FCLEngine.h" +#ifdef HAVE_BULLET_COLLISION + #include "dart/collision/bullet/BulletCollisionObjectData.h" +#endif + +namespace dart { +namespace collision { + +//============================================================================== +Option::Option(bool enableContact, size_t maxNumContacts) + : enableContact(enableContact), + maxNumContacts(maxNumContacts) +{ + // Do nothing +} + +//============================================================================== +void Result::clear() +{ + contacts.clear(); +} + +//============================================================================= +bool Result::empty() const +{ + return contacts.empty(); +} + +//============================================================================= +CollisionObjectData* Engine::createCollisionObjectData( + EngineType type, + CollisionObject* parent, + const dynamics::ShapePtr& shape) +{ + switch (type) + { + case FCL: + return FCLEngine::createCollisionObjectData(parent, shape); + break; + default: + dtwarn << "Unsupported collision detection engine '" << type << "'.\n"; + return nullptr; + break; + } +} + +//============================================================================== +bool Engine::detect(CollisionObject* object1, + CollisionObject* object2, + const Option& option, Result& result) +{ + if (!object1 || !object2) + { + result.clear(); + return false; + } + + auto engineType1 = object1->getEngineType(); + auto engineType2 = object2->getEngineType(); + + if (engineType1 != engineType2) + { + result.clear(); + return false; + } + // TODO(JS): we can consider changing one engine of the two objects, or + // cloning one object whose engine is the same with other's. + + switch (engineType1) + { + case FCL: + return FCLEngine::detect(object1, object2, option, result); + break; + default: + dtwarn << "Unsupported collision detection engine '" << engineType1 << "'.\n"; + return nullptr; + break; + } +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h new file mode 100644 index 0000000000000..919d3579684a0 --- /dev/null +++ b/dart/collision/Engine.h @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_ENGINE_H_ +#define DART_COLLISION_ENGINE_H_ + +#include +#include + +#include + +#include "dart/config.h" +#include "dart/common/Console.h" +#include "dart/collision/Contact.h" +#include "dart/collision/CollisionGroup.h" +#include "dart/collision/CollisionDetector.h" +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace collision { + +class CollisionGroup; +class CollisionObject; +class CollisionObjectData; + +enum EngineType +{ + FCL = 0, +#ifdef HAVE_BULLET_COLLISION + Bullet, +#endif + DART, +}; + +struct Option +{ + bool enableContact; + size_t maxNumContacts; + + Option(bool enableContact = true, + size_t maxNumContacts = 100); +}; + +struct Result +{ + std::vector contacts; + + void clear(); + + bool empty() const; +}; + +namespace Engine +{ + +CollisionObjectData* createCollisionObjectData(EngineType type, + CollisionObject* parent, + const dynamics::ShapePtr& shape); + +bool detect(CollisionObject* object1, + CollisionObject* object2, + const collision::Option& option, + collision::Result& result); + +} + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_ENGINE_H_ diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 874a9cb8e9a7e..452adc1fb7881 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -257,7 +257,7 @@ CollisionNode* FCLCollisionDetector::findCollisionNode( FCLCollisionNode* FCLCollisionDetector::findCollisionNode( const fcl::CollisionObject* _fclCollObj) const { - FCLUserData* userData = static_cast(_fclCollObj->getUserData()); + FCLCollisionGeometryUserData* userData = static_cast(_fclCollObj->getUserData()); return userData->fclCollNode; } diff --git a/dart/collision/fcl/FCLCollisionNode.cpp b/dart/collision/fcl/FCLCollisionNode.cpp index b68c34244acf2..e148e16565f91 100644 --- a/dart/collision/fcl/FCLCollisionNode.cpp +++ b/dart/collision/fcl/FCLCollisionNode.cpp @@ -58,6 +58,49 @@ namespace dart { namespace collision { +//============================================================================== +// Create a cube mesh for collision detection +template +fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) +{ + int faces[6][4] = + { + {0, 1, 2, 3}, + {3, 2, 6, 7}, + {7, 6, 5, 4}, + {4, 5, 1, 0}, + {5, 6, 2, 1}, + {7, 4, 0, 3} + }; + float v[8][3]; + + v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; + v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; + v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; + v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; + v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; + v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 6; i++) + { + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + model->addTriangle(p1, p2, p3); + + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + model->addTriangle(p1, p2, p3); + } + model->endModel(); + return model; +} + //============================================================================== template fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) @@ -265,6 +308,109 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) return model; } +//============================================================================== +template +fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, + double _height, int _slices, int _stacks) +{ + const int CACHE_SIZE = 240; + + int i, j; + float sinCache[CACHE_SIZE]; + float cosCache[CACHE_SIZE]; + float angle; + float zBase; + float zLow, zHigh; + float sintemp, costemp; + float deltaRadius; + float radiusLow, radiusHigh; + + if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; + + if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || + _height < 0.0) + { + return nullptr; + } + + /* Center at CoM */ + zBase = -_height/2; + + /* Compute delta */ + deltaRadius = _baseRadius - _topRadius; + + /* Cache is the vertex locations cache */ + for (i = 0; i < _slices; i++) + { + angle = 2 * M_PI * i / _slices; + sinCache[i] = sin(angle); + cosCache[i] = cos(angle); + } + + sinCache[_slices] = sinCache[0]; + cosCache[_slices] = cosCache[0]; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3, p4; + + model->beginModel(); + + /* Base of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _baseRadius; + zLow = zBase; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + /* Body of cylinder */ + for (i = 0; i < _slices; i++) + { + for (j = 0; j < _stacks; j++) + { + zLow = j * _height / _stacks + zBase; + zHigh = (j + 1) * _height / _stacks + zBase; + radiusLow = _baseRadius + - deltaRadius * (static_cast(j) / _stacks); + radiusHigh = _baseRadius + - deltaRadius * (static_cast(j + 1) / _stacks); + + p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); + + model->addTriangle(p1, p2, p3); + model->addTriangle(p2, p3, p4); + } + } + + /* Top of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _topRadius; + zLow = zBase + _height; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + return model; +} + //============================================================================== template fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, @@ -298,8 +444,7 @@ fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, //============================================================================== template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh, - const fcl::Transform3f& _transform) +fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) { // Create FCL mesh from Assimp mesh @@ -312,10 +457,8 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh, fcl::Vec3f vertices[3]; for (size_t j = 0; j < 3; j++) { - const aiVector3D& vertex - = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; + const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - vertices[j] = _transform.transform(vertices[j]); } model->addTriangle(vertices[0], vertices[1], vertices[2]); } @@ -325,12 +468,23 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh, } //============================================================================== -FCLUserData::FCLUserData(FCLCollisionNode* _fclCollNode, - dynamics::BodyNode* _bodyNode, - dynamics::Shape* _shape) - : fclCollNode(_fclCollNode), - bodyNode(_bodyNode), - shape(_shape) +FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( + FCLCollisionNode* fclCollNode, + dynamics::BodyNode* bodyNode, + const dynamics::ShapePtr& shape) + : fclCollNode(fclCollNode), + bodyNode(bodyNode), + shape(shape) +{ + // Do nothing +} + +//============================================================================== +FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( + CollisionObject* collisionObject, + const dynamics::ShapePtr& shape) + : collisionObject(collisionObject), + shape(shape) { // Do nothing } @@ -360,8 +514,11 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) assert(dynamic_cast(shape.get())); const BoxShape* box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(size[0], size[1], size[2])); +#else fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); - +#endif break; } case Shape::ELLIPSOID: @@ -377,14 +534,14 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) } else { -#ifdef FCL_DART5 - fclCollGeom.reset( - new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); -#else +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset( createEllipsoid(ellipsoid->getSize()[0], ellipsoid->getSize()[1], ellipsoid->getSize()[2])); +#else + fclCollGeom.reset( + new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); #endif } @@ -393,22 +550,32 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) case Shape::CYLINDER: { assert(dynamic_cast(shape.get())); - CylinderShape* cylinder = static_cast(shape.get()); + const CylinderShape* cylinder + = static_cast(shape.get()); const double radius = cylinder->getRadius(); const double height = cylinder->getHeight(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); +#else fclCollGeom.reset(new fcl::Cylinder(radius, height)); - +#endif + // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 + // returns single contact point for cylinder yet. break; } case Shape::PLANE: { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); +#else assert(dynamic_cast(shape.get())); dynamics::PlaneShape* plane = static_cast(shape.get()); const Eigen::Vector3d normal = plane->getNormal(); const double offset = plane->getOffset(); fclCollGeom.reset( new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); - +#endif break; } case Shape::MESH: @@ -423,19 +590,15 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) break; } -#if 0 case Shape::SOFT_MESH: { assert(dynamic_cast(shape.get())); SoftMeshShape* softMeshShape = static_cast(shape.get()); fclCollGeom.reset( - createSoftMesh( - softMeshShape->getAssimpMesh(), - FCLTypes::convertTransform(Eigen::Isometry3d::Identity()))); + createSoftMesh(softMeshShape->getAssimpMesh())); break; } -#endif default: { dterr << "[FCLCollisionNode::FCLCollisionNode] Attempting to create " @@ -448,7 +611,7 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) assert(nullptr != fclCollGeom); fcl::CollisionObject* fclCollObj = new fcl::CollisionObject(fclCollGeom, getFCLTransform(i)); - fclCollObj->setUserData(new FCLUserData(this, _bodyNode, shape.get())); + fclCollGeom->setUserData(new FCLCollisionGeometryUserData(this, _bodyNode, shape)); mCollisionObjects.push_back(fclCollObj); } } @@ -460,7 +623,7 @@ FCLCollisionNode::~FCLCollisionNode() { assert(collObj); - delete static_cast(collObj->getUserData()); + delete static_cast(collObj->getUserData()); delete collObj; } } @@ -471,6 +634,13 @@ size_t FCLCollisionNode::getNumCollisionObjects() const return mCollisionObjects.size(); } +//============================================================================== +const FCLCollisionNode::FCLCollisionObjects& +FCLCollisionNode::getCollisionObjects() +{ + return mCollisionObjects; +} + //============================================================================== fcl::CollisionObject* FCLCollisionNode::getCollisionObject(size_t _idx) const { @@ -501,17 +671,11 @@ void FCLCollisionNode::updateFCLCollisionObjects() for (auto& fclCollObj : mCollisionObjects) { - FCLUserData* userData - = static_cast(fclCollObj->getUserData()); + FCLCollisionGeometryUserData* userData + = static_cast(fclCollObj->collisionGeometry()->getUserData()); BodyNode* bodyNode = userData->bodyNode; - Shape* shape = userData->shape; - - // Update shape's transform - const Eigen::Isometry3d W = bodyNode->getWorldTransform() - * shape->getLocalTransform(); - fclCollObj->setTransform(FCLTypes::convertTransform(W)); - fclCollObj->computeAABB(); + Shape* shape = userData->shape.get(); // Update soft-body's vertices if (shape->getShapeType() == Shape::SOFT_MESH) @@ -548,6 +712,12 @@ void FCLCollisionNode::updateFCLCollisionObjects() } bvhModel->endUpdateModel(); } + + // Update shape's transform + const Eigen::Isometry3d W = bodyNode->getWorldTransform() + * shape->getLocalTransform(); + fclCollObj->setTransform(FCLTypes::convertTransform(W)); + fclCollObj->computeAABB(); } } diff --git a/dart/collision/fcl/FCLCollisionNode.h b/dart/collision/fcl/FCLCollisionNode.h index ce39fbb7ffb51..2d6e191cd74ec 100644 --- a/dart/collision/fcl/FCLCollisionNode.h +++ b/dart/collision/fcl/FCLCollisionNode.h @@ -58,22 +58,47 @@ namespace dart { namespace collision { class FCLCollisionNode; +class CollisionObject; -struct FCLUserData +struct FCLCollisionGeometryUserData { FCLCollisionNode* fclCollNode; + CollisionObject* collisionObject; dynamics::BodyNode* bodyNode; - dynamics::Shape* shape; + dynamics::ShapePtr shape; - FCLUserData(FCLCollisionNode* _fclCollNode, - dynamics::BodyNode* _bodyNode, - dynamics::Shape* _shape); + FCLCollisionGeometryUserData(FCLCollisionNode* fclCollNode, + dynamics::BodyNode* bodyNode, + const dynamics::ShapePtr& shape); + + FCLCollisionGeometryUserData(CollisionObject* fclCollNode, + const dynamics::ShapePtr& shape); + +}; + +class ConstFCLCollisionObjects +{ +public: + using ConstIterator = std::vector::const_iterator; + + ConstFCLCollisionObjects(const std::vector& container) + : mContainer(container) + {} + + ConstIterator begin() const { return mContainer.cbegin(); } + ConstIterator end() const; + +protected: + std::vector mContainer; }; /// FCLCollisionNode class FCLCollisionNode : public CollisionNode { public: + + using FCLCollisionObjects = std::vector; + /// Constructor explicit FCLCollisionNode(dynamics::BodyNode* _bodyNode); @@ -83,6 +108,8 @@ class FCLCollisionNode : public CollisionNode /// Get number of collision objects size_t getNumCollisionObjects() const; + const FCLCollisionObjects& getCollisionObjects(); + /// Get FCL collision object given index fcl::CollisionObject* getCollisionObject(size_t _idx) const; @@ -93,8 +120,10 @@ class FCLCollisionNode : public CollisionNode void updateFCLCollisionObjects(); private: + /// Array of FCL collision object that continas geometry and transform - std::vector mCollisionObjects; + FCLCollisionObjects mCollisionObjects; + }; } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionObjectData.cpp b/dart/collision/fcl/FCLCollisionObjectData.cpp new file mode 100644 index 0000000000000..f239f2ab4ca8e --- /dev/null +++ b/dart/collision/fcl/FCLCollisionObjectData.cpp @@ -0,0 +1,622 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/fcl/FCLCollisionObjectData.h" + +#include +#include +#include + +#include "dart/common/Console.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/Engine.h" +#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" + +#define FCL_VERSION_AT_LEAST(x,y,z) \ + (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \ + (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \ + FCL_PATCH_VERSION >= z)))) + +namespace dart { +namespace collision { + +namespace { + +//============================================================================== +// Create a cube mesh for collision detection +template +fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) +{ + int faces[6][4] = + { + {0, 1, 2, 3}, + {3, 2, 6, 7}, + {7, 6, 5, 4}, + {4, 5, 1, 0}, + {5, 6, 2, 1}, + {7, 4, 0, 3} + }; + float v[8][3]; + + v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; + v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; + v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; + v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; + v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; + v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 6; i++) + { + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + model->addTriangle(p1, p2, p3); + + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + model->addTriangle(p1, p2, p3); + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) +{ + float v[59][3] = + { + {0, 0, 0}, + {0.135299, -0.461940, -0.135299}, + {0.000000, -0.461940, -0.191342}, + {-0.135299, -0.461940, -0.135299}, + {-0.191342, -0.461940, 0.000000}, + {-0.135299, -0.461940, 0.135299}, + {0.000000, -0.461940, 0.191342}, + {0.135299, -0.461940, 0.135299}, + {0.191342, -0.461940, 0.000000}, + {0.250000, -0.353553, -0.250000}, + {0.000000, -0.353553, -0.353553}, + {-0.250000, -0.353553, -0.250000}, + {-0.353553, -0.353553, 0.000000}, + {-0.250000, -0.353553, 0.250000}, + {0.000000, -0.353553, 0.353553}, + {0.250000, -0.353553, 0.250000}, + {0.353553, -0.353553, 0.000000}, + {0.326641, -0.191342, -0.326641}, + {0.000000, -0.191342, -0.461940}, + {-0.326641, -0.191342, -0.326641}, + {-0.461940, -0.191342, 0.000000}, + {-0.326641, -0.191342, 0.326641}, + {0.000000, -0.191342, 0.461940}, + {0.326641, -0.191342, 0.326641}, + {0.461940, -0.191342, 0.000000}, + {0.353553, 0.000000, -0.353553}, + {0.000000, 0.000000, -0.500000}, + {-0.353553, 0.000000, -0.353553}, + {-0.500000, 0.000000, 0.000000}, + {-0.353553, 0.000000, 0.353553}, + {0.000000, 0.000000, 0.500000}, + {0.353553, 0.000000, 0.353553}, + {0.500000, 0.000000, 0.000000}, + {0.326641, 0.191342, -0.326641}, + {0.000000, 0.191342, -0.461940}, + {-0.326641, 0.191342, -0.326641}, + {-0.461940, 0.191342, 0.000000}, + {-0.326641, 0.191342, 0.326641}, + {0.000000, 0.191342, 0.461940}, + {0.326641, 0.191342, 0.326641}, + {0.461940, 0.191342, 0.000000}, + {0.250000, 0.353553, -0.250000}, + {0.000000, 0.353553, -0.353553}, + {-0.250000, 0.353553, -0.250000}, + {-0.353553, 0.353553, 0.000000}, + {-0.250000, 0.353553, 0.250000}, + {0.000000, 0.353553, 0.353553}, + {0.250000, 0.353553, 0.250000}, + {0.353553, 0.353553, 0.000000}, + {0.135299, 0.461940, -0.135299}, + {0.000000, 0.461940, -0.191342}, + {-0.135299, 0.461940, -0.135299}, + {-0.191342, 0.461940, 0.000000}, + {-0.135299, 0.461940, 0.135299}, + {0.000000, 0.461940, 0.191342}, + {0.135299, 0.461940, 0.135299}, + {0.191342, 0.461940, 0.000000}, + {0.000000, -0.500000, 0.000000}, + {0.000000, 0.500000, 0.000000} + }; + + int f[112][3] = + { + {1, 2, 9}, + {9, 2, 10}, + {2, 3, 10}, + {10, 3, 11}, + {3, 4, 11}, + {11, 4, 12}, + {4, 5, 12}, + {12, 5, 13}, + {5, 6, 13}, + {13, 6, 14}, + {6, 7, 14}, + {14, 7, 15}, + {7, 8, 15}, + {15, 8, 16}, + {8, 1, 16}, + {16, 1, 9}, + {9, 10, 17}, + {17, 10, 18}, + {10, 11, 18}, + {18, 11, 19}, + {11, 12, 19}, + {19, 12, 20}, + {12, 13, 20}, + {20, 13, 21}, + {13, 14, 21}, + {21, 14, 22}, + {14, 15, 22}, + {22, 15, 23}, + {15, 16, 23}, + {23, 16, 24}, + {16, 9, 24}, + {24, 9, 17}, + {17, 18, 25}, + {25, 18, 26}, + {18, 19, 26}, + {26, 19, 27}, + {19, 20, 27}, + {27, 20, 28}, + {20, 21, 28}, + {28, 21, 29}, + {21, 22, 29}, + {29, 22, 30}, + {22, 23, 30}, + {30, 23, 31}, + {23, 24, 31}, + {31, 24, 32}, + {24, 17, 32}, + {32, 17, 25}, + {25, 26, 33}, + {33, 26, 34}, + {26, 27, 34}, + {34, 27, 35}, + {27, 28, 35}, + {35, 28, 36}, + {28, 29, 36}, + {36, 29, 37}, + {29, 30, 37}, + {37, 30, 38}, + {30, 31, 38}, + {38, 31, 39}, + {31, 32, 39}, + {39, 32, 40}, + {32, 25, 40}, + {40, 25, 33}, + {33, 34, 41}, + {41, 34, 42}, + {34, 35, 42}, + {42, 35, 43}, + {35, 36, 43}, + {43, 36, 44}, + {36, 37, 44}, + {44, 37, 45}, + {37, 38, 45}, + {45, 38, 46}, + {38, 39, 46}, + {46, 39, 47}, + {39, 40, 47}, + {47, 40, 48}, + {40, 33, 48}, + {48, 33, 41}, + {41, 42, 49}, + {49, 42, 50}, + {42, 43, 50}, + {50, 43, 51}, + {43, 44, 51}, + {51, 44, 52}, + {44, 45, 52}, + {52, 45, 53}, + {45, 46, 53}, + {53, 46, 54}, + {46, 47, 54}, + {54, 47, 55}, + {47, 48, 55}, + {55, 48, 56}, + {48, 41, 56}, + {56, 41, 49}, + {2, 1, 57}, + {3, 2, 57}, + {4, 3, 57}, + {5, 4, 57}, + {6, 5, 57}, + {7, 6, 57}, + {8, 7, 57}, + {1, 8, 57}, + {49, 50, 58}, + {50, 51, 58}, + {51, 52, 58}, + {52, 53, 58}, + {53, 54, 58}, + {54, 55, 58}, + {55, 56, 58}, + {56, 49, 58} + }; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 112; i++) + { + p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, + v[f[i][0]][1] * _sizeY, + v[f[i][0]][2] * _sizeZ); + p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, + v[f[i][1]][1] * _sizeY, + v[f[i][1]][2] * _sizeZ); + p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, + v[f[i][2]][1] * _sizeY, + v[f[i][2]][2] * _sizeZ); + + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + + return model; +} + +//============================================================================== +template +fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, + double _height, int _slices, int _stacks) +{ + const int CACHE_SIZE = 240; + + int i, j; + float sinCache[CACHE_SIZE]; + float cosCache[CACHE_SIZE]; + float angle; + float zBase; + float zLow, zHigh; + float sintemp, costemp; + float deltaRadius; + float radiusLow, radiusHigh; + + if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; + + if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || + _height < 0.0) + { + return nullptr; + } + + /* Center at CoM */ + zBase = -_height/2; + + /* Compute delta */ + deltaRadius = _baseRadius - _topRadius; + + /* Cache is the vertex locations cache */ + for (i = 0; i < _slices; i++) + { + angle = 2 * M_PI * i / _slices; + sinCache[i] = sin(angle); + cosCache[i] = cos(angle); + } + + sinCache[_slices] = sinCache[0]; + cosCache[_slices] = cosCache[0]; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3, p4; + + model->beginModel(); + + /* Base of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _baseRadius; + zLow = zBase; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + /* Body of cylinder */ + for (i = 0; i < _slices; i++) + { + for (j = 0; j < _stacks; j++) + { + zLow = j * _height / _stacks + zBase; + zHigh = (j + 1) * _height / _stacks + zBase; + radiusLow = _baseRadius + - deltaRadius * (static_cast(j) / _stacks); + radiusHigh = _baseRadius + - deltaRadius * (static_cast(j + 1) / _stacks); + + p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); + + model->addTriangle(p1, p2, p3); + model->addTriangle(p2, p3, p4); + } + } + + /* Top of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _topRadius; + zLow = zBase + _height; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, + const aiScene* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + for (size_t i = 0; i < _mesh->mNumMeshes; i++) + { + for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) + { + fcl::Vec3f vertices[3]; + for (size_t k = 0; k < 3; k++) + { + const aiVector3D& vertex + = _mesh->mMeshes[i]->mVertices[ + _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + vertices[k] = fcl::Vec3f(vertex.x * _scaleX, + vertex.y * _scaleY, + vertex.z * _scaleZ); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + + for (size_t i = 0; i < _mesh->mNumFaces; i++) + { + fcl::Vec3f vertices[3]; + for (size_t j = 0; j < 3; j++) + { + const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; + vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + model->endModel(); + return model; +} + +} // anonymous namespace + +//============================================================================== +FCLCollisionObjectData::FCLCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) + : CollisionObjectData(), + mFCLCollisionGeometryUserData( + new FCLCollisionGeometryUserData(parent, shape)) +{ + updateShape(shape); +} + +//============================================================================== +void FCLCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) +{ + mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); + mFCLCollisionObject->computeAABB(); +} + +//============================================================================== +void FCLCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) +{ + using dynamics::Shape; + using dynamics::BoxShape; + using dynamics::EllipsoidShape; + using dynamics::CylinderShape; + using dynamics::PlaneShape; + using dynamics::MeshShape; + using dynamics::SoftMeshShape; + + boost::shared_ptr fclCollGeom; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + const BoxShape* box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(size[0], size[1], size[2])); +#else + fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); +#endif + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + EllipsoidShape* ellipsoid = static_cast(shape.get()); + + const Eigen::Vector3d& size = ellipsoid->getSize(); + + if (ellipsoid->isSphere()) + { + fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); + } + else + { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset( + createEllipsoid(ellipsoid->getSize()[0], + ellipsoid->getSize()[1], + ellipsoid->getSize()[2])); +#else + fclCollGeom.reset( + new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); +#endif + } + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + const CylinderShape* cylinder + = static_cast(shape.get()); + const double radius = cylinder->getRadius(); + const double height = cylinder->getHeight(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); +#else + fclCollGeom.reset(new fcl::Cylinder(radius, height)); +#endif + // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 + // returns single contact point for cylinder yet. + break; + } + case Shape::PLANE: + { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); +#else + assert(dynamic_cast(shape.get())); + dynamics::PlaneShape* plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + fclCollGeom.reset( + new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); +#endif + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + MeshShape* shapeMesh = static_cast(shape.get()); + fclCollGeom.reset( + createMesh(shapeMesh->getScale()[0], + shapeMesh->getScale()[1], + shapeMesh->getScale()[2], + shapeMesh->getMesh())); + + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + SoftMeshShape* softMeshShape = static_cast(shape.get()); + fclCollGeom.reset( + createSoftMesh(softMeshShape->getAssimpMesh())); + + break; + } + default: + { + dterr << "[FCLCollisionObjectData::createFCLCollisionObject] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + return; + } + } + + assert(fclCollGeom); + fclCollGeom->setUserData(mFCLCollisionGeometryUserData.get()); + + mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); +} + +//============================================================================== +fcl::CollisionObject* FCLCollisionObjectData::getFCLCollisionObject() const +{ + return mFCLCollisionObject.get(); +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/fcl/FCLCollisionObjectData.h b/dart/collision/fcl/FCLCollisionObjectData.h new file mode 100644 index 0000000000000..88fcd6c1623a2 --- /dev/null +++ b/dart/collision/fcl/FCLCollisionObjectData.h @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ + +#include +#include + +#include + +#include "dart/collision/CollisionObjectData.h" + +namespace dart { +namespace collision { + +class CollisionObject; +class FCLCollisionGeometryUserData; + +//struct FCLCollisionObjectUserData +//{ +// CollisionObject* collisionObject; +// dynamics::ShapePtr shape; + +// FCLCollisionObjectUserData(CollisionObject* collisionObject, +// const dynamics::ShapePtr& shape) +// : collisionObject(collisionObject), +// shape(shape) {} +//}; + +class FCLCollisionObjectData : public CollisionObjectData +{ +public: + + /// Constructor + FCLCollisionObjectData(CollisionObject* parent, + const dynamics::ShapePtr& shape); + + // Documentation inherited + void updateTransform(const Eigen::Isometry3d& tf) override; + + // Documentation inherited + void updateShape(const dynamics::ShapePtr& shape) override; + + fcl::CollisionObject* getFCLCollisionObject() const; + +protected: + + /// FCL collision object + std::unique_ptr mFCLCollisionObject; + + std::unique_ptr mFCLCollisionGeometryUserData; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp new file mode 100644 index 0000000000000..632d91afb0ac0 --- /dev/null +++ b/dart/collision/fcl/FCLEngine.cpp @@ -0,0 +1,239 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/fcl/FCLEngine.h" + +#include + +#include +#include +#include + +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/collision/fcl/FCLCollisionGroup.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" + +namespace dart { +namespace collision { + +namespace { + +// Collision data stores the collision request and the result given by +// collision algorithm. +struct CollisionData +{ + + // Collision request + fcl::CollisionRequest request; + + // Collision result + fcl::CollisionResult result; + + // FCL collision detector +// FCLEngine* collisionDetector; + + // Whether the collision iteration can stop + bool done; + + bool checkAllCollisions; + + CollisionData(); +}; + +bool collisionCallBack(fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata); + +void convert(const Option& fclResult, fcl::CollisionRequest& request); + +void convert(const fcl::CollisionResult& fclResult, Result& result); + +void convert(const fcl::Contact& fclContact, Contact& contact); + +} // anonymous namespace + + + +//============================================================================== +CollisionObjectData* FCLEngine::createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) +{ + return new FCLCollisionObjectData(parent, shape); +} + +//============================================================================== +bool FCLEngine::detect(CollisionObject* object1, + CollisionObject* object2, + const Option& option, + Result& result) +{ + result.clear(); + + assert(object1->getEngineType() == FCL); + assert(object2->getEngineType() == FCL); + + auto data1 = static_cast(object1->getEngineData()); + auto data2 = static_cast(object2->getEngineData()); + + auto fclCollObj1 = data1->getFCLCollisionObject(); + auto fclCollObj2 = data2->getFCLCollisionObject(); + + object1->updateEngineData(); + object2->updateEngineData(); + + CollisionData collData; + convert(option, collData.request); + + collisionCallBack(fclCollObj1, fclCollObj2, &collData); + + convert(collData.result, result); + + return !result.empty(); +} + + + +namespace { + +//============================================================================== +CollisionData::CollisionData() +{ + done = false; +} + +//============================================================================== +bool collisionCallBack(fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata) +{ + CollisionData* collData = static_cast(cdata); + + const fcl::CollisionRequest& request = collData->request; + fcl::CollisionResult& result = collData->result; +// FCLEngine* cd = collData->collisionDetector; + // TODO(JS): take filter object instead of collision detector + + if (collData->done) + return true; + + // Filtering +// if (!cd->isCollidable(cd->findCollisionNode(o1), cd->findCollisionNode(o2))) +// return collData->done; + // TODO(JS): disabled until other functionalities are implemented + + // Perform narrow-phase detection + fcl::collide(o1, o2, request, result); + + if (!request.enable_cost + && (result.isCollision()) + && ((result.numContacts() >= request.num_max_contacts) + || !collData->checkAllCollisions)) + { + collData->done = true; + } + + return collData->done; +} + +//============================================================================== +void convert(const Option& fclResult, fcl::CollisionRequest& request) +{ + request.num_max_contacts = fclResult.maxNumContacts; + request.enable_contact = fclResult.enableContact; +#if FCL_VERSION_AT_LEAST(0,3,0) + request.gjk_solver_type = fcl::GST_LIBCCD; +#endif +} + +//============================================================================== +void convert(const fcl::CollisionResult& fclResult, Result& result) +{ + result.contacts.resize(fclResult.numContacts()); + + // TODO(JS): Check if there is contact point sufficiently close to the new + // contact point + + for (auto i = 0u; i < result.contacts.size(); ++i) + convert(fclResult.getContact(i), result.contacts[i]); +} + +//============================================================================== +void convert(const fcl::Contact& fclContact, Contact& contact) +{ + Eigen::Vector3d point = FCLTypes::convertVector3(fclContact.pos); + + contact.point = point; + contact.normal = -FCLTypes::convertVector3(fclContact.normal); + contact.penetrationDepth = fclContact.penetration_depth; + contact.triID1 = fclContact.b1; + contact.triID2 = fclContact.b2; + + FCLCollisionGeometryUserData* userData1 + = static_cast(fclContact.o1->getUserData()); + FCLCollisionGeometryUserData* userData2 + = static_cast(fclContact.o2->getUserData()); + assert(userData1); + assert(userData2); + contact.shape1 = userData1->shape; + contact.shape2 = userData2->shape; + contact.collisionObject1 = userData1->collisionObject; + contact.collisionObject2 = userData2->collisionObject; + +// mContacts.push_back(contact); + + // Set these two bodies are in colliding + // contact.bodyNode1.lock()->setColliding(true); + // contact.bodyNode2.lock()->setColliding(true); + // TODO(JS): really need this? +} + +} // anonymous namespace + +} // namespace collision +} // namespace dart diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h new file mode 100644 index 0000000000000..7469aa4ce36dc --- /dev/null +++ b/dart/collision/fcl/FCLEngine.h @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_FCLENGINE_H_ +#define DART_COLLISION_FCL_FCLENGINE_H_ + +#include "dart/collision/Engine.h" + +namespace dart { +namespace collision { + +class FCLCollisionGroup; + +/// FCL Collidion detection engine +namespace FCLEngine +{ + +CollisionObjectData* createCollisionObjectData(CollisionObject* parent, + const dynamics::ShapePtr& shape); + +bool detect(CollisionObject* object1, + CollisionObject* object2, + const collision::Option& option, + collision::Result& result); + +} + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_FCLEngine_H_ diff --git a/dart/collision/fcl/FCLTypes.h b/dart/collision/fcl/FCLTypes.h index ddf931cb4047c..d901b2b41cdf6 100644 --- a/dart/collision/fcl/FCLTypes.h +++ b/dart/collision/fcl/FCLTypes.h @@ -42,6 +42,15 @@ #include #include +#define FCL_VERSION_AT_LEAST(x,y,z) \ + (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \ + (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \ + FCL_PATCH_VERSION >= z)))) + +#define FCL_MAJOR_MINOR_VERSION_AT_MOST(x,y) \ + (FCL_MAJOR_VERSION < x || (FCL_MAJOR_VERSION <= x && \ + (FCL_MINOR_VERSION < y || (FCL_MINOR_VERSION <= y)))) + namespace dart { namespace collision { diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 00242f58f7805..59d1a2a166584 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -504,6 +504,40 @@ TEST_F(COLLISION, FCL_BOX_BOX) //} +//============================================================================== +TEST_F(COLLISION, FreeCollisionObject) +{ + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + + collision::FreeCollisionObject obj1(shape1); + collision::FreeCollisionObject obj2(shape2); + + obj1.setTranslation(Eigen::Vector3d::Zero()); + obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + + collision::Option option; + collision::Result result; + + EXPECT_FALSE(obj1.detect(&obj2, option, result)); + + obj2.setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); + EXPECT_TRUE(obj1.detect(&obj2, option, result)); + + EXPECT_EQ(result.contacts.size(), 4u); + + for (auto contact : result.contacts) + { + auto freeCollObj1 = dynamic_cast( + contact.collisionObject1); + auto freeCollObj2 = dynamic_cast( + contact.collisionObject2); + + EXPECT_NE(freeCollObj1, nullptr); + EXPECT_NE(freeCollObj2, nullptr); + } +} + //============================================================================== TEST_F(COLLISION, CollisionOfPrescribedJoints) { diff --git a/unittests/testConstraint.cpp b/unittests/testConstraint.cpp index c9761faf8de53..501333ba395a2 100644 --- a/unittests/testConstraint.cpp +++ b/unittests/testConstraint.cpp @@ -158,7 +158,7 @@ void ConstraintTest::SingleContactTest(const std::string& _fileName) EXPECT_EQ((int)world->getNumSkeletons(), 2); ConstraintSolver* cs = world->getConstraintSolver(); - CollisionDetector* cd = cs->getCollisionDetector(); + dart::collision::CollisionDetector* cd = cs->getCollisionDetector(); // Lower and upper bound of configuration for system // double lb = -1.5 * DART_PI; From 0b2da84a59207de7491fa3bc5ec31dfb00b01b43 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 15 Feb 2016 17:41:39 -0500 Subject: [PATCH 02/67] Add group-group collision detection functionality --- dart/collision/CollisionGroup.cpp | 117 +++++++++++++++++ dart/collision/CollisionGroup.h | 99 +++++++++++++++ ...ionObjectData.cpp => CollisionGroupData.h} | 24 +++- dart/collision/CollisionObject.cpp | 28 ++-- dart/collision/CollisionObject.h | 20 ++- dart/collision/Engine.cpp | 52 -------- dart/collision/Engine.h | 43 ++++--- dart/collision/fcl/FCLCollisionGroupData.cpp | 91 +++++++++++++ dart/collision/fcl/FCLCollisionGroupData.h | 79 ++++++++++++ dart/collision/fcl/FCLCollisionObjectData.h | 11 -- dart/collision/fcl/FCLEngine.cpp | 120 ++++++++++++++++-- dart/collision/fcl/FCLEngine.h | 36 ++++-- dart/dynamics/CollisionGroupGenerator.cpp | 56 ++++++++ dart/dynamics/CollisionGroupGenerator.h | 72 +++++++++++ unittests/testCollision.cpp | 95 +++++++++++++- 15 files changed, 811 insertions(+), 132 deletions(-) create mode 100644 dart/collision/CollisionGroup.cpp create mode 100644 dart/collision/CollisionGroup.h rename dart/collision/{CollisionObjectData.cpp => CollisionGroupData.h} (78%) create mode 100644 dart/collision/fcl/FCLCollisionGroupData.cpp create mode 100644 dart/collision/fcl/FCLCollisionGroupData.h create mode 100644 dart/dynamics/CollisionGroupGenerator.cpp create mode 100644 dart/dynamics/CollisionGroupGenerator.h diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp new file mode 100644 index 0000000000000..f2fc83c16af22 --- /dev/null +++ b/dart/collision/CollisionGroup.cpp @@ -0,0 +1,117 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/CollisionGroup.h" + +#include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionGroupData.h" + +#include + +namespace dart { +namespace collision { + +//============================================================================== +CollisionGroup::CollisionGroup( + Engine* engine, + const CollisionGroup::CollisionObjects& collObjects) + : mEngine(engine) +{ + assert(mEngine); + + mCollisionObjects = collObjects; + + mEngineData.reset(mEngine->createCollisionGroupData(mCollisionObjects)); +} + +//============================================================================== +CollisionGroup::~CollisionGroup() +{ +} + +//============================================================================== +Engine* CollisionGroup::getEngine() const +{ + return mEngine; +} + +//============================================================================== +CollisionGroupData* CollisionGroup::getEngineData() +{ + return mEngineData.get(); +} + +//============================================================================== +void CollisionGroup::updateEngineData() +{ + for (auto collObj : mCollisionObjects) + collObj->updateEngineData(); + + mEngineData->update(); +} + +//============================================================================== +void CollisionGroup::addCollisionObject(CollisionObject* object) +{ + auto found = std::find(mCollisionObjects.begin(), + mCollisionObjects.end(), object) + != mCollisionObjects.end(); + + if (found) + { + // TODO(JS): print warning message + return; + } + + mCollisionObjects.push_back(object); + mEngineData->notifyCollisionObjectAdded(object); +} + +//============================================================================== +bool CollisionGroup::detect(const Option& option, Result& result) +{ + return mEngine->detect(this, option, result); +} + +//============================================================================== +bool CollisionGroup::detect(CollisionGroup* otherGroup, + const Option& option, Result& result) +{ + return mEngine->detect(this, otherGroup, option, result); +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h new file mode 100644 index 0000000000000..2355b07802549 --- /dev/null +++ b/dart/collision/CollisionGroup.h @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_COLLISIONGROUP_H_ +#define DART_COLLISION_COLLISIONGROUP_H_ + +#include + +#include "dart/collision/CollisionNode.h" +#include "dart/collision/Engine.h" +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace collision { + +class Engine; +class CollisionNode; +class CollisionGroupData; + +class CollisionGroup +{ +public: + + using CollisionObjects = std::vector; + + /// Default constructor + CollisionGroup( + Engine* engine, + const CollisionObjects& collObjects = CollisionObjects()); + + /// Default destructor + virtual ~CollisionGroup(); + + Engine* getEngine() const; + + CollisionGroupData* getEngineData(); + + void updateEngineData(); + + void addCollisionObject(CollisionObject* object); + +// virtual void addCollisionNode(const CollisionNodes& nodes); + + bool detect(const Option& option, Result& result); + + bool detect(CollisionGroup* group, const Option& option, Result& result); + +protected: + + /// Collision engine + Engine* mEngine; + + CollisionObjects mCollisionObjects; + + /// Engine specific data + std::shared_ptr mEngineData; + +}; +// TODO(JS): make this class iterable for collision objects + +using CollisionGroupPtr = std::shared_ptr; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONGROUP_H_ diff --git a/dart/collision/CollisionObjectData.cpp b/dart/collision/CollisionGroupData.h similarity index 78% rename from dart/collision/CollisionObjectData.cpp rename to dart/collision/CollisionGroupData.h index c05a6161e4903..8a6f32b6a1e16 100644 --- a/dart/collision/CollisionObjectData.cpp +++ b/dart/collision/CollisionGroupData.h @@ -34,12 +34,32 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/CollisionObjectData.h" +#ifndef DART_COLLISION_COLLISIONGROUPDATA_H_ +#define DART_COLLISION_COLLISIONGROUPDATA_H_ -#include "dart/dynamics/BodyNode.h" +#include + +#include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { +class CollisionObject; + +class CollisionGroupData +{ +public: + virtual void update() = 0; + + virtual void notifyCollisionObjectAdded(CollisionObject* object) = 0; + + // TODO(JS): notifyCollisionObjectUpdated()? + + virtual void notifyCollisionObjectRemoved(CollisionObject* object) = 0; + +}; + } // namespace collision } // namespace dart + +#endif // DART_COLLISION_COLLISIONGROUPDATA_H_ diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 5b0ad6703b787..b8f6942261b27 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -54,9 +54,9 @@ dynamics::ShapePtr CollisionObject::getShape() const //} //============================================================================== -EngineType CollisionObject::getEngineType() const +Engine* CollisionObject::getEngine() const { - return mEngineType; + return mEngine; } //============================================================================== @@ -76,23 +76,27 @@ bool CollisionObject::detect(CollisionObject* other, const Option& option, Result& result) { - return Engine::detect(this, other, option, result); + return mEngine->detect(this, other, option, result); } //============================================================================== -CollisionObject::CollisionObject(const dynamics::ShapePtr& shape, - EngineType type) - : mShape(shape), - mEngineType(type) +CollisionObject::CollisionObject(Engine* engine, + const dynamics::ShapePtr& shape) + : mEngine(engine), + mShape(shape) { - mEngineData.reset( - Engine::createCollisionObjectData(mEngineType, this, mShape)); + assert(mEngine); + assert(mShape); + + mEngineData.reset(mEngine->createCollisionObjectData(this, mShape)); } //============================================================================== -FreeCollisionObject::FreeCollisionObject(const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf, EngineType type) - : CollisionObject(shape, type), - mW(Eigen::Isometry3d::Identity()) +FreeCollisionObject::FreeCollisionObject(Engine* engine, + const dynamics::ShapePtr& shape, + const Eigen::Isometry3d& tf) + : CollisionObject(engine, shape), + mW(tf) { // Do nothing } diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 1428cf6fa0c29..55eaffccdef6d 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -40,7 +40,6 @@ #include #include -#include "dart/collision/Contact.h" #include "dart/collision/Engine.h" #include "dart/collision/CollisionObjectData.h" @@ -61,7 +60,7 @@ class CollisionObject // virtual void reportCollision(); - EngineType getEngineType() const; + Engine* getEngine() const; // virtual void changeEngine(EngineType type) {} @@ -74,9 +73,7 @@ class CollisionObject /// Perform collision detection with other CollisionObject. Return false if /// the engine type of the other CollisionObject is different from this /// CollisionObject's. - bool detect(CollisionObject* other, - const collision::Option& option, - collision::Result& result); + bool detect(CollisionObject* other, const Option& option, Result& result); // bool detect(CollisionGroup* group, // const collision::Option& option, @@ -85,16 +82,16 @@ class CollisionObject protected: /// Contructor - CollisionObject(const dynamics::ShapePtr& shape, EngineType type); + CollisionObject(Engine* engine, const dynamics::ShapePtr& shape); protected: + /// Collision engine + Engine* mEngine; + /// Shape dynamics::ShapePtr mShape; - /// Collision engine type - EngineType mEngineType; - /// Engine specific data std::shared_ptr mEngineData; @@ -108,9 +105,10 @@ class FreeCollisionObject : public CollisionObject /// Constructor FreeCollisionObject( + Engine* engine, const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), - EngineType type = FCL); + const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); + // TODO(JS): change to engine pointer /// Set world transformation of this FreeCollisionObject void setTransform(const Eigen::Isometry3d& tf); diff --git a/dart/collision/Engine.cpp b/dart/collision/Engine.cpp index 3ce87ddf65a13..be318dbab3a62 100644 --- a/dart/collision/Engine.cpp +++ b/dart/collision/Engine.cpp @@ -74,57 +74,5 @@ bool Result::empty() const return contacts.empty(); } -//============================================================================= -CollisionObjectData* Engine::createCollisionObjectData( - EngineType type, - CollisionObject* parent, - const dynamics::ShapePtr& shape) -{ - switch (type) - { - case FCL: - return FCLEngine::createCollisionObjectData(parent, shape); - break; - default: - dtwarn << "Unsupported collision detection engine '" << type << "'.\n"; - return nullptr; - break; - } -} - -//============================================================================== -bool Engine::detect(CollisionObject* object1, - CollisionObject* object2, - const Option& option, Result& result) -{ - if (!object1 || !object2) - { - result.clear(); - return false; - } - - auto engineType1 = object1->getEngineType(); - auto engineType2 = object2->getEngineType(); - - if (engineType1 != engineType2) - { - result.clear(); - return false; - } - // TODO(JS): we can consider changing one engine of the two objects, or - // cloning one object whose engine is the same with other's. - - switch (engineType1) - { - case FCL: - return FCLEngine::detect(object1, object2, option, result); - break; - default: - dtwarn << "Unsupported collision detection engine '" << engineType1 << "'.\n"; - return nullptr; - break; - } -} - } // namespace collision } // namespace dart diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h index 919d3579684a0..8ef9cce241141 100644 --- a/dart/collision/Engine.h +++ b/dart/collision/Engine.h @@ -44,8 +44,7 @@ #include "dart/config.h" #include "dart/common/Console.h" -#include "dart/collision/Contact.h" -#include "dart/collision/CollisionGroup.h" +//#include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionDetector.h" #include "dart/dynamics/SmartPointer.h" @@ -55,15 +54,7 @@ namespace collision { class CollisionGroup; class CollisionObject; class CollisionObjectData; - -enum EngineType -{ - FCL = 0, -#ifdef HAVE_BULLET_COLLISION - Bullet, -#endif - DART, -}; +class CollisionGroupData; struct Option { @@ -83,19 +74,31 @@ struct Result bool empty() const; }; -namespace Engine +class Engine { +public: + + virtual const std::string& getType() const = 0; -CollisionObjectData* createCollisionObjectData(EngineType type, - CollisionObject* parent, - const dynamics::ShapePtr& shape); + virtual CollisionObjectData* createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) = 0; + // TODO(JS): shared_ptr -bool detect(CollisionObject* object1, - CollisionObject* object2, - const collision::Option& option, - collision::Result& result); + virtual CollisionGroupData* createCollisionGroupData( + std::vector collObjects) = 0; + // TODO(JS): shared_ptr -} + virtual bool detect(CollisionObject* object1, CollisionObject* object2, + const Option& option, Result& result) = 0; + + virtual bool detect(CollisionGroup* group, + const Option& option, Result& result) = 0; + + virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) = 0; + +}; } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLCollisionGroupData.cpp b/dart/collision/fcl/FCLCollisionGroupData.cpp new file mode 100644 index 0000000000000..5973f8b56fd0d --- /dev/null +++ b/dart/collision/fcl/FCLCollisionGroupData.cpp @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/fcl/FCLCollisionGroupData.h" + +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" + +namespace dart { +namespace collision { + +//============================================================================== +FCLCollisionGroupData::FCLCollisionGroupData( + const FCLCollisionGroupData::CollisionObjects& collObjects) + : mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +{ + for (auto collObj : collObjects) + { + auto data = static_cast(collObj->getEngineData()); + mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + } + + mBroadPhaseAlg->setup(); +} + +//============================================================================== +void FCLCollisionGroupData::update() +{ + mBroadPhaseAlg->update(); +} + +//============================================================================== +void FCLCollisionGroupData::notifyCollisionObjectAdded(CollisionObject* object) +{ + auto data = static_cast(object->getEngineData()); + mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + mBroadPhaseAlg->setup(); +} + +//============================================================================== +void FCLCollisionGroupData::notifyCollisionObjectRemoved( + CollisionObject* object) +{ + auto data = static_cast(object->getEngineData()); + mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); + mBroadPhaseAlg->setup(); +} + +//============================================================================== +FCLCollisionGroupData::FCLCollisionManager* +FCLCollisionGroupData::getFCLCollisionManager() const +{ + return mBroadPhaseAlg.get(); +} + + +} // namespace collision +} // namespace dart diff --git a/dart/collision/fcl/FCLCollisionGroupData.h b/dart/collision/fcl/FCLCollisionGroupData.h new file mode 100644 index 0000000000000..38346b9e1c7d5 --- /dev/null +++ b/dart/collision/fcl/FCLCollisionGroupData.h @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ + +#include + +#include "dart/collision/CollisionGroup.h" +#include "dart/collision/CollisionGroupData.h" + +namespace dart { +namespace collision { + +class CollisionObject; +class FCLCollisionGeometryUserData; + +class FCLCollisionGroupData : public CollisionGroupData +{ +public: + + using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; + using CollisionObjects = CollisionGroup::CollisionObjects; + + /// Constructor + FCLCollisionGroupData(const CollisionObjects& collObjects); + + void update() override; + + void notifyCollisionObjectAdded(CollisionObject* object) override; + + void notifyCollisionObjectRemoved(CollisionObject* object) override; + + FCLCollisionManager* getFCLCollisionManager() const; + +protected: + + /// Broad-phase collision checker of FCL + std::unique_ptr mBroadPhaseAlg; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ diff --git a/dart/collision/fcl/FCLCollisionObjectData.h b/dart/collision/fcl/FCLCollisionObjectData.h index 88fcd6c1623a2..f2cb5566f51c7 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.h +++ b/dart/collision/fcl/FCLCollisionObjectData.h @@ -50,17 +50,6 @@ namespace collision { class CollisionObject; class FCLCollisionGeometryUserData; -//struct FCLCollisionObjectUserData -//{ -// CollisionObject* collisionObject; -// dynamics::ShapePtr shape; - -// FCLCollisionObjectUserData(CollisionObject* collisionObject, -// const dynamics::ShapePtr& shape) -// : collisionObject(collisionObject), -// shape(shape) {} -//}; - class FCLCollisionObjectData : public CollisionObjectData { public: diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index 632d91afb0ac0..68a4a56689b52 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -46,8 +46,9 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" #include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLCollisionNode.h" -#include "dart/collision/fcl/FCLCollisionGroup.h" +#include "dart/collision/fcl/FCLCollisionGroupData.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" #include "dart/dynamics/Shape.h" @@ -99,6 +100,19 @@ void convert(const fcl::Contact& fclContact, Contact& contact); +//============================================================================== +const std::string& FCLEngine::getType() const +{ + return getTypeStatic(); +} + +//============================================================================== +const std::string& FCLEngine::getTypeStatic() +{ + static const std::string& type("FCL"); + return type; +} + //============================================================================== CollisionObjectData* FCLEngine::createCollisionObjectData( CollisionObject* parent, @@ -107,6 +121,13 @@ CollisionObjectData* FCLEngine::createCollisionObjectData( return new FCLCollisionObjectData(parent, shape); } +//============================================================================== +CollisionGroupData* FCLEngine::createCollisionGroupData( + std::vector collObjects) +{ + return new FCLCollisionGroupData(collObjects); +} + //============================================================================== bool FCLEngine::detect(CollisionObject* object1, CollisionObject* object2, @@ -115,8 +136,11 @@ bool FCLEngine::detect(CollisionObject* object1, { result.clear(); - assert(object1->getEngineType() == FCL); - assert(object2->getEngineType() == FCL); + assert(object1->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(object2->getEngine()->getType() == FCLEngine::getTypeStatic()); + + object1->updateEngineData(); + object2->updateEngineData(); auto data1 = static_cast(object1->getEngineData()); auto data2 = static_cast(object2->getEngineData()); @@ -124,9 +148,6 @@ bool FCLEngine::detect(CollisionObject* object1, auto fclCollObj1 = data1->getFCLCollisionObject(); auto fclCollObj2 = data2->getFCLCollisionObject(); - object1->updateEngineData(); - object2->updateEngineData(); - CollisionData collData; convert(option, collData.request); @@ -137,6 +158,86 @@ bool FCLEngine::detect(CollisionObject* object1, return !result.empty(); } +//============================================================================== +bool FCLEngine::detect(CollisionGroup* group, + const Option& option, Result& result) +{ + result.clear(); + + assert(group); + assert(group->getEngine()->getType() == FCLEngine::getTypeStatic()); + + group->updateEngineData(); + + auto data = static_cast(group->getEngineData()); + + auto broadPhaseAlg = data->getFCLCollisionManager(); + + CollisionData collData; + convert(option, collData.request); + + broadPhaseAlg->collide(&collData, collisionCallBack); + + convert(collData.result, result); + + return !result.empty(); +} + +//============================================================================== +bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) +{ + result.clear(); + + assert(group1); + assert(group2); + assert(group1->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(group2->getEngine()->getType() == FCLEngine::getTypeStatic()); + + group1->updateEngineData(); + group2->updateEngineData(); + + auto data1 = static_cast(group1->getEngineData()); + auto data2 = static_cast(group2->getEngineData()); + + auto broadPhaseAlg1 = data1->getFCLCollisionManager(); + auto broadPhaseAlg2 = data2->getFCLCollisionManager(); + + CollisionData collData; + convert(option, collData.request); + + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); + + convert(collData.result, result); + + return !result.empty(); +} + +//============================================================================== +//bool FCLEngine::detect(CollisionGroup* group, +// const Option& option, Result& result) +//{ +// result.clear(); + +// assert(group->getEngineType() == FCL); + +// auto data = static_cast(group->getEngineData()); + +// auto broadPhaseAlg = data->getFCLBroadPhaseAlg(); + +//// object1->updateEngineData(); +//// object2->updateEngineData(); + +// CollisionData collData; +// convert(option, collData.request); + +//// collisionCallBack(fclCollObj1, fclCollObj2, &collData); + +// convert(collData.result, result); + +// return !result.empty(); +//} + namespace { @@ -224,13 +325,6 @@ void convert(const fcl::Contact& fclContact, Contact& contact) contact.shape2 = userData2->shape; contact.collisionObject1 = userData1->collisionObject; contact.collisionObject2 = userData2->collisionObject; - -// mContacts.push_back(contact); - - // Set these two bodies are in colliding - // contact.bodyNode1.lock()->setColliding(true); - // contact.bodyNode2.lock()->setColliding(true); - // TODO(JS): really need this? } } // anonymous namespace diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index 7469aa4ce36dc..6d8faee92a1b4 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -45,18 +45,38 @@ namespace collision { class FCLCollisionGroup; /// FCL Collidion detection engine -namespace FCLEngine +class FCLEngine : public Engine { +public: -CollisionObjectData* createCollisionObjectData(CollisionObject* parent, - const dynamics::ShapePtr& shape); + /// Return engine type "FCL" + static const std::string& getTypeStatic(); -bool detect(CollisionObject* object1, - CollisionObject* object2, - const collision::Option& option, - collision::Result& result); + // Documentation inherit + const std::string& getType() const override; -} + // Documentation inherit + CollisionObjectData* createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) override; + + // Documentation inherit + CollisionGroupData* createCollisionGroupData( + std::vector collObjects) override; + + // Documentation inherit + bool detect(CollisionObject* object1, CollisionObject* object2, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroup* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; + +}; } // namespace collision } // namespace dart diff --git a/dart/dynamics/CollisionGroupGenerator.cpp b/dart/dynamics/CollisionGroupGenerator.cpp new file mode 100644 index 0000000000000..e8b11b5acc612 --- /dev/null +++ b/dart/dynamics/CollisionGroupGenerator.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/CollisionGroupGenerator.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +//collision::CollisionGroupPtr CollisionGroupGenerator::generate( +// const collision::CollisionDetectorPtr& cd, +// const SkeletonPtr& skeleton) +//{ +// // get list of collision node +// std::vector collisionNodes; + +// collision::CollisionGroupPtr group = cd->createGroup(collisionNodes); + +// return group; +//} + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/CollisionGroupGenerator.h b/dart/dynamics/CollisionGroupGenerator.h new file mode 100644 index 0000000000000..f2dc98613957a --- /dev/null +++ b/dart/dynamics/CollisionGroupGenerator.h @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_COLLISIONGROUPGENERATOR_H_ +#define DART_DYNAMICS_COLLISIONGROUPGENERATOR_H_ + +#include + +#include "dart/collision/CollisionDetector.h" +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace dynamics { + +namespace CollisionNodeGenerator +{ +// collision::CollisionNode2* create(const BodyNodePtr& bodyNode); +} + +namespace CollisionGroupGenerator +{ +// collision::CollisionGroupPtr generate( +// const collision::CollisionDetectorPtr& cd, +// const SkeletonPtr& skeleton); + +// CollisionGroupPtr +// createGroup(const ShapeNodePair& shape) { return nullptr; } + +// CollisionGroupPtr +// createGroup(const dynamics::BodyNodePtr& bodyNode) { return nullptr; } + +// CollisionGroupPtr +// createGroup(const dynamics::SkeletonPtr& Skeleton) { return nullptr; } +} + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_NODE_H_ diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 59d1a2a166584..2e52ac172e203 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -45,7 +45,8 @@ #include "dart/common/common.h" #include "dart/math/math.h" #include "dart/dynamics/dynamics.h" -//#include "dart/collision/unc/UNCCollisionDetector.h" +#include "dart/collision/CollisionGroup.h" +#include "dart/collision/fcl/FCLEngine.h" #include "dart/simulation/simulation.h" #include "dart/utils/utils.h" @@ -507,11 +508,13 @@ TEST_F(COLLISION, FCL_BOX_BOX) //============================================================================== TEST_F(COLLISION, FreeCollisionObject) { + collision::FCLEngine fclEngine; + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - collision::FreeCollisionObject obj1(shape1); - collision::FreeCollisionObject obj2(shape2); + collision::FreeCollisionObject obj1(&fclEngine, shape1); + collision::FreeCollisionObject obj2(&fclEngine, shape2); obj1.setTranslation(Eigen::Vector3d::Zero()); obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); @@ -538,6 +541,92 @@ TEST_F(COLLISION, FreeCollisionObject) } } +//============================================================================== +TEST_F(COLLISION, FreeCollisionGroup) +{ + collision::FCLEngine fclEngine; + + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + + collision::FreeCollisionObject obj1(&fclEngine, shape1); + collision::FreeCollisionObject obj2(&fclEngine, shape2); + + collision::CollisionGroup group(&fclEngine); + group.addCollisionObject(&obj1); + group.addCollisionObject(&obj2); + + obj1.setTranslation(Eigen::Vector3d::Zero()); + obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + + collision::Option option; + collision::Result result; + + EXPECT_FALSE(group.detect(option, result)); + + obj2.setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); + EXPECT_TRUE(group.detect(option, result)); + + EXPECT_EQ(result.contacts.size(), 4u); + + for (auto contact : result.contacts) + { + auto freeCollObj1 = dynamic_cast( + contact.collisionObject1); + auto freeCollObj2 = dynamic_cast( + contact.collisionObject2); + + EXPECT_NE(freeCollObj1, nullptr); + EXPECT_NE(freeCollObj2, nullptr); + } +} + +//============================================================================== +TEST_F(COLLISION, FreeCollisionGroupGroup) +{ + collision::FCLEngine fclEngine; + + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + + collision::FreeCollisionObject obj1(&fclEngine, shape1); + collision::FreeCollisionObject obj2(&fclEngine, shape2); + collision::FreeCollisionObject obj3(&fclEngine, shape3); + + collision::CollisionGroup group1(&fclEngine); + group1.addCollisionObject(&obj1); + group1.addCollisionObject(&obj2); + + collision::CollisionGroup group2(&fclEngine); + group2.addCollisionObject(&obj3); + + obj1.setTranslation(Eigen::Vector3d::Zero()); + obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + obj3.setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); + + collision::Option option; + collision::Result result; + + EXPECT_FALSE(group1.detect(&group2, option, result)); + + obj3.setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); + EXPECT_TRUE(group1.detect(&group2, option, result)); + + EXPECT_EQ(result.contacts.size(), 4u); + + for (auto contact : result.contacts) + { + auto freeCollObj1 = dynamic_cast( + contact.collisionObject1); + auto freeCollObj2 = dynamic_cast( + contact.collisionObject2); + + EXPECT_NE(freeCollObj1, nullptr); + EXPECT_NE(freeCollObj2, nullptr); + } +} + //============================================================================== TEST_F(COLLISION, CollisionOfPrescribedJoints) { From a0b1305efa1fe15cc2edce236706d5f373ed8ed1 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 15 Feb 2016 18:18:29 -0500 Subject: [PATCH 03/67] Add object-group collision detection functionality --- dart/collision/CollisionGroup.cpp | 39 ++++--- dart/collision/CollisionGroup.h | 32 ++++-- ...GroupData.h => CollisionGroupEngineData.h} | 12 +- dart/collision/CollisionObject.cpp | 33 +++--- dart/collision/CollisionObject.h | 37 +++--- ...jectData.h => CollisionObjectEngineData.h} | 8 +- dart/collision/Engine.cpp | 26 +---- dart/collision/Engine.h | 39 ++++--- ...ta.cpp => FCLCollisionGroupEngineData.cpp} | 24 ++-- ...upData.h => FCLCollisionGroupEngineData.h} | 12 +- ...a.cpp => FCLCollisionObjectEngineData.cpp} | 17 +-- ...tData.h => FCLCollisionObjectEngineData.h} | 12 +- dart/collision/fcl/FCLEngine.cpp | 106 ++++++++---------- dart/collision/fcl/FCLEngine.h | 10 +- unittests/testCollision.cpp | 43 +++++++ 15 files changed, 249 insertions(+), 201 deletions(-) rename dart/collision/{CollisionGroupData.h => CollisionGroupEngineData.h} (90%) rename dart/collision/{CollisionObjectData.h => CollisionObjectEngineData.h} (91%) rename dart/collision/fcl/{FCLCollisionGroupData.cpp => FCLCollisionGroupEngineData.cpp} (77%) rename dart/collision/fcl/{FCLCollisionGroupData.h => FCLCollisionGroupEngineData.h} (87%) rename dart/collision/fcl/{FCLCollisionObjectData.cpp => FCLCollisionObjectEngineData.cpp} (97%) rename dart/collision/fcl/{FCLCollisionObjectData.h => FCLCollisionObjectEngineData.h} (87%) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index f2fc83c16af22..23cd8530f8cc6 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -37,7 +37,7 @@ #include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionGroupData.h" +#include "dart/collision/CollisionGroupEngineData.h" #include @@ -68,21 +68,6 @@ Engine* CollisionGroup::getEngine() const return mEngine; } -//============================================================================== -CollisionGroupData* CollisionGroup::getEngineData() -{ - return mEngineData.get(); -} - -//============================================================================== -void CollisionGroup::updateEngineData() -{ - for (auto collObj : mCollisionObjects) - collObj->updateEngineData(); - - mEngineData->update(); -} - //============================================================================== void CollisionGroup::addCollisionObject(CollisionObject* object) { @@ -106,6 +91,13 @@ bool CollisionGroup::detect(const Option& option, Result& result) return mEngine->detect(this, option, result); } +//============================================================================== +bool CollisionGroup::detect(CollisionObject* object, + const Option& option, Result& result) +{ + return mEngine->detect(object, this, option, result); +} + //============================================================================== bool CollisionGroup::detect(CollisionGroup* otherGroup, const Option& option, Result& result) @@ -113,5 +105,20 @@ bool CollisionGroup::detect(CollisionGroup* otherGroup, return mEngine->detect(this, otherGroup, option, result); } +//============================================================================== +CollisionGroupEngineData* CollisionGroup::getEngineData() +{ + return mEngineData.get(); +} + +//============================================================================== +void CollisionGroup::updateEngineData() +{ + for (auto collObj : mCollisionObjects) + collObj->updateEngineData(); + + mEngineData->update(); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 2355b07802549..86d3effeedbd6 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -39,16 +39,14 @@ #include -#include "dart/collision/CollisionNode.h" #include "dart/collision/Engine.h" -#include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { class Engine; class CollisionNode; -class CollisionGroupData; +class CollisionGroupEngineData; class CollisionGroup { @@ -64,29 +62,47 @@ class CollisionGroup /// Default destructor virtual ~CollisionGroup(); + /// Return collision detection engine associated with this CollisionGroup Engine* getEngine() const; - CollisionGroupData* getEngineData(); - - void updateEngineData(); - + /// Add collision object to this CollisionGroup void addCollisionObject(CollisionObject* object); // virtual void addCollisionNode(const CollisionNodes& nodes); + /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); + /// Perform collision detection with other CollisionObject. + /// + /// Return false if the engine type of the other CollisionObject is different + /// from this CollisionObject engine. + bool detect(CollisionObject* object, const Option& option, Result& result); + + /// Perform collision detection with other CollisionGroup. + /// + /// Return false if the engine type of the other CollisionGroup is different + /// from this CollisionObject engine. bool detect(CollisionGroup* group, const Option& option, Result& result); + /// Return the collision detection engine specific data of this + /// CollisionObject + CollisionGroupEngineData* getEngineData(); + + /// Update engine data. This function should be called before the collision + /// detection is performed by the engine in most cases. + void updateEngineData(); + protected: /// Collision engine Engine* mEngine; + /// Collision objects CollisionObjects mCollisionObjects; /// Engine specific data - std::shared_ptr mEngineData; + std::shared_ptr mEngineData; }; // TODO(JS): make this class iterable for collision objects diff --git a/dart/collision/CollisionGroupData.h b/dart/collision/CollisionGroupEngineData.h similarity index 90% rename from dart/collision/CollisionGroupData.h rename to dart/collision/CollisionGroupEngineData.h index 8a6f32b6a1e16..c8c5eebb74b4a 100644 --- a/dart/collision/CollisionGroupData.h +++ b/dart/collision/CollisionGroupEngineData.h @@ -34,19 +34,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_COLLISIONGROUPDATA_H_ -#define DART_COLLISION_COLLISIONGROUPDATA_H_ - -#include - -#include "dart/dynamics/SmartPointer.h" +#ifndef DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ +#define DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ namespace dart { namespace collision { class CollisionObject; -class CollisionGroupData +class CollisionGroupEngineData { public: virtual void update() = 0; @@ -62,4 +58,4 @@ class CollisionGroupData } // namespace collision } // namespace dart -#endif // DART_COLLISION_COLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index b8f6942261b27..e126993adb722 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -36,11 +36,17 @@ #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObjectEngineData.h" namespace dart { namespace collision { +//============================================================================== +Engine* CollisionObject::getEngine() const +{ + return mEngine; +} + //============================================================================== dynamics::ShapePtr CollisionObject::getShape() const { @@ -48,19 +54,22 @@ dynamics::ShapePtr CollisionObject::getShape() const } //============================================================================== -//void CollisionObject::reportCollision() -//{ -// // Do nothing -//} +bool CollisionObject::detect(CollisionObject* other, + const Option& option, + Result& result) +{ + return mEngine->detect(this, other, option, result); +} //============================================================================== -Engine* CollisionObject::getEngine() const +bool CollisionObject::detect(CollisionGroup* group, + const Option& option, Result& result) { - return mEngine; + return mEngine->detect(this, group, option, result); } //============================================================================== -CollisionObjectData* CollisionObject::getEngineData() const +CollisionObjectEngineData* CollisionObject::getEngineData() const { return mEngineData.get(); } @@ -71,14 +80,6 @@ void CollisionObject::updateEngineData() mEngineData->updateTransform(getTransform()); } -//============================================================================== -bool CollisionObject::detect(CollisionObject* other, - const Option& option, - Result& result) -{ - return mEngine->detect(this, other, option, result); -} - //============================================================================== CollisionObject::CollisionObject(Engine* engine, const dynamics::ShapePtr& shape) diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 55eaffccdef6d..a45b703bcc395 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -37,48 +37,49 @@ #ifndef DART_COLLISION_COLLISIONOBJECT_H_ #define DART_COLLISION_COLLISIONOBJECT_H_ -#include #include #include "dart/collision/Engine.h" -#include "dart/collision/CollisionObjectData.h" namespace dart { namespace collision { -class CollisionObjectData; +class CollisionObjectEngineData; class CollisionObject { public: - virtual const Eigen::Isometry3d getTransform() const = 0; + /// Return collision detection engine associated with this CollisionObject + Engine* getEngine() const; /// Return shape pointer that associated with this CollisionObject dynamics::ShapePtr getShape() const; // TODO(JS): Shape should be in common or math - // virtual void reportCollision(); + /// Return the transformation of this CollisionObject in world coordinates + virtual const Eigen::Isometry3d getTransform() const = 0; - Engine* getEngine() const; + /// Perform collision detection with other CollisionObject. + /// + /// Return false if the engine type of the other CollisionObject is different + /// from this CollisionObject engine. + bool detect(CollisionObject* other, const Option& option, Result& result); - // virtual void changeEngine(EngineType type) {} + /// Perform collision detection with other CollisionGroup. + /// + /// Return false if the engine type of the other CollisionGroup is different + /// from this CollisionObject engine. + bool detect(CollisionGroup* group, const Option& option, Result& result); - CollisionObjectData* getEngineData() const; + /// Return the collision detection engine specific data of this + /// CollisionObject + CollisionObjectEngineData* getEngineData() const; /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void updateEngineData(); - /// Perform collision detection with other CollisionObject. Return false if - /// the engine type of the other CollisionObject is different from this - /// CollisionObject's. - bool detect(CollisionObject* other, const Option& option, Result& result); - - // bool detect(CollisionGroup* group, - // const collision::Option& option, - // collision::Result& result); - protected: /// Contructor @@ -93,7 +94,7 @@ class CollisionObject dynamics::ShapePtr mShape; /// Engine specific data - std::shared_ptr mEngineData; + std::shared_ptr mEngineData; }; diff --git a/dart/collision/CollisionObjectData.h b/dart/collision/CollisionObjectEngineData.h similarity index 91% rename from dart/collision/CollisionObjectData.h rename to dart/collision/CollisionObjectEngineData.h index 1f85abc52c04e..499be890686ce 100644 --- a/dart/collision/CollisionObjectData.h +++ b/dart/collision/CollisionObjectEngineData.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_COLLISIONOBJECTDATA_H_ -#define DART_COLLISION_COLLISIONOBJECTDATA_H_ +#ifndef DART_COLLISION_COLLISIONOBJECTENGINEDATA_H_ +#define DART_COLLISION_COLLISIONOBJECTENGINEDATA_H_ #include @@ -44,7 +44,7 @@ namespace dart { namespace collision { -class CollisionObjectData +class CollisionObjectEngineData { public: @@ -57,4 +57,4 @@ class CollisionObjectData } // namespace collision } // namespace dart -#endif // DART_COLLISION_COLLISIONOBJECTDATA_H_ +#endif // DART_COLLISION_COLLISIONOBJECTENGINEDATA_H_ diff --git a/dart/collision/Engine.cpp b/dart/collision/Engine.cpp index be318dbab3a62..b52e869232eee 100644 --- a/dart/collision/Engine.cpp +++ b/dart/collision/Engine.cpp @@ -36,21 +36,6 @@ #include "dart/collision/Engine.h" -#include -#include -#include - -#include "dart/common/Console.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Skeleton.h" -#include "dart/collision/CollisionNode.h" -#include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionObjectData.h" -#include "dart/collision/fcl/FCLEngine.h" -#ifdef HAVE_BULLET_COLLISION - #include "dart/collision/bullet/BulletCollisionObjectData.h" -#endif - namespace dart { namespace collision { @@ -62,16 +47,11 @@ Option::Option(bool enableContact, size_t maxNumContacts) // Do nothing } -//============================================================================== -void Result::clear() -{ - contacts.clear(); -} - //============================================================================= -bool Result::empty() const +bool Engine::detect(CollisionGroup* group, CollisionObject* object, + const Option& option, Result& result) { - return contacts.empty(); + return detect(object, group, option, result); } } // namespace collision diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h index 8ef9cce241141..c44cdd21ffa20 100644 --- a/dart/collision/Engine.h +++ b/dart/collision/Engine.h @@ -38,13 +38,7 @@ #define DART_COLLISION_ENGINE_H_ #include -#include -#include - -#include "dart/config.h" -#include "dart/common/Console.h" -//#include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionDetector.h" #include "dart/dynamics/SmartPointer.h" @@ -52,49 +46,66 @@ namespace dart { namespace collision { class CollisionGroup; +class CollisionGroupEngineData; class CollisionObject; -class CollisionObjectData; -class CollisionGroupData; +class CollisionObjectEngineData; struct Option { + /// Flag whether compute contact information such as point, normal, and + /// penetration depth. If this flag is set to false, the Engine returns only + /// simple information whether there is a collision of not. bool enableContact; + + /// Maximum number of contacts to detect size_t maxNumContacts; + /// Constructor Option(bool enableContact = true, size_t maxNumContacts = 100); }; struct Result { + /// List of contact information for each contact std::vector contacts; - - void clear(); - - bool empty() const; }; class Engine { public: + /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; - virtual CollisionObjectData* createCollisionObjectData( + /// Create collision detection engine specific data for CollisionObject + virtual CollisionObjectEngineData* createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) = 0; // TODO(JS): shared_ptr - virtual CollisionGroupData* createCollisionGroupData( + /// Create collision detection engine specific data for CollisionGroup + virtual CollisionGroupEngineData* createCollisionGroupData( std::vector collObjects) = 0; // TODO(JS): shared_ptr + /// Perform collision detection for object1-object2. virtual bool detect(CollisionObject* object1, CollisionObject* object2, const Option& option, Result& result) = 0; + /// Perform collision detection for object-group. + virtual bool detect(CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result) = 0; + + /// Identical with detect(object, group, option, result) + bool detect(CollisionGroup* group, CollisionObject* object, + const Option& option, Result& result); + + /// Perform collision detection for group. virtual bool detect(CollisionGroup* group, const Option& option, Result& result) = 0; + /// Perform collision detection for group1-group2. virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) = 0; diff --git a/dart/collision/fcl/FCLCollisionGroupData.cpp b/dart/collision/fcl/FCLCollisionGroupEngineData.cpp similarity index 77% rename from dart/collision/fcl/FCLCollisionGroupData.cpp rename to dart/collision/fcl/FCLCollisionGroupEngineData.cpp index 5973f8b56fd0d..353f8d0eabcfa 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.cpp +++ b/dart/collision/fcl/FCLCollisionGroupEngineData.cpp @@ -34,22 +34,22 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLCollisionGroupData.h" +#include "dart/collision/fcl/FCLCollisionGroupEngineData.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObjectEngineData.h" namespace dart { namespace collision { //============================================================================== -FCLCollisionGroupData::FCLCollisionGroupData( - const FCLCollisionGroupData::CollisionObjects& collObjects) +FCLCollisionGroupEngineData::FCLCollisionGroupEngineData( + const FCLCollisionGroupEngineData::CollisionObjects& collObjects) : mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { for (auto collObj : collObjects) { - auto data = static_cast(collObj->getEngineData()); + auto data = static_cast(collObj->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); } @@ -57,31 +57,31 @@ FCLCollisionGroupData::FCLCollisionGroupData( } //============================================================================== -void FCLCollisionGroupData::update() +void FCLCollisionGroupEngineData::update() { mBroadPhaseAlg->update(); } //============================================================================== -void FCLCollisionGroupData::notifyCollisionObjectAdded(CollisionObject* object) +void FCLCollisionGroupEngineData::notifyCollisionObjectAdded(CollisionObject* object) { - auto data = static_cast(object->getEngineData()); + auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); mBroadPhaseAlg->setup(); } //============================================================================== -void FCLCollisionGroupData::notifyCollisionObjectRemoved( +void FCLCollisionGroupEngineData::notifyCollisionObjectRemoved( CollisionObject* object) { - auto data = static_cast(object->getEngineData()); + auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); mBroadPhaseAlg->setup(); } //============================================================================== -FCLCollisionGroupData::FCLCollisionManager* -FCLCollisionGroupData::getFCLCollisionManager() const +FCLCollisionGroupEngineData::FCLCollisionManager* +FCLCollisionGroupEngineData::getFCLCollisionManager() const { return mBroadPhaseAlg.get(); } diff --git a/dart/collision/fcl/FCLCollisionGroupData.h b/dart/collision/fcl/FCLCollisionGroupEngineData.h similarity index 87% rename from dart/collision/fcl/FCLCollisionGroupData.h rename to dart/collision/fcl/FCLCollisionGroupEngineData.h index 38346b9e1c7d5..18ada46fc06de 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.h +++ b/dart/collision/fcl/FCLCollisionGroupEngineData.h @@ -34,13 +34,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUPENGINEDATA_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONGROUPENGINEDATA_H_ #include #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupData.h" +#include "dart/collision/CollisionGroupEngineData.h" namespace dart { namespace collision { @@ -48,7 +48,7 @@ namespace collision { class CollisionObject; class FCLCollisionGeometryUserData; -class FCLCollisionGroupData : public CollisionGroupData +class FCLCollisionGroupEngineData : public CollisionGroupEngineData { public: @@ -56,7 +56,7 @@ class FCLCollisionGroupData : public CollisionGroupData using CollisionObjects = CollisionGroup::CollisionObjects; /// Constructor - FCLCollisionGroupData(const CollisionObjects& collObjects); + FCLCollisionGroupEngineData(const CollisionObjects& collObjects); void update() override; @@ -76,4 +76,4 @@ class FCLCollisionGroupData : public CollisionGroupData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUPENGINEDATA_H_ diff --git a/dart/collision/fcl/FCLCollisionObjectData.cpp b/dart/collision/fcl/FCLCollisionObjectEngineData.cpp similarity index 97% rename from dart/collision/fcl/FCLCollisionObjectData.cpp rename to dart/collision/fcl/FCLCollisionObjectEngineData.cpp index f239f2ab4ca8e..6e3e18583f8f8 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObjectEngineData.h" #include #include @@ -53,11 +53,6 @@ #include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" -#define FCL_VERSION_AT_LEAST(x,y,z) \ - (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \ - (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \ - FCL_PATCH_VERSION >= z)))) - namespace dart { namespace collision { @@ -475,10 +470,10 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) } // anonymous namespace //============================================================================== -FCLCollisionObjectData::FCLCollisionObjectData( +FCLCollisionObjectEngineData::FCLCollisionObjectEngineData( CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectData(), + : CollisionObjectEngineData(), mFCLCollisionGeometryUserData( new FCLCollisionGeometryUserData(parent, shape)) { @@ -486,14 +481,14 @@ FCLCollisionObjectData::FCLCollisionObjectData( } //============================================================================== -void FCLCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) +void FCLCollisionObjectEngineData::updateTransform(const Eigen::Isometry3d& tf) { mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); mFCLCollisionObject->computeAABB(); } //============================================================================== -void FCLCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) +void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) { using dynamics::Shape; using dynamics::BoxShape; @@ -613,7 +608,7 @@ void FCLCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) } //============================================================================== -fcl::CollisionObject* FCLCollisionObjectData::getFCLCollisionObject() const +fcl::CollisionObject* FCLCollisionObjectEngineData::getFCLCollisionObject() const { return mFCLCollisionObject.get(); } diff --git a/dart/collision/fcl/FCLCollisionObjectData.h b/dart/collision/fcl/FCLCollisionObjectEngineData.h similarity index 87% rename from dart/collision/fcl/FCLCollisionObjectData.h rename to dart/collision/fcl/FCLCollisionObjectEngineData.h index f2cb5566f51c7..cb0cf4ca9391d 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.h +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.h @@ -34,15 +34,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECTENGINEDATA_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONOBJECTENGINEDATA_H_ #include #include #include -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObjectEngineData.h" namespace dart { namespace collision { @@ -50,12 +50,12 @@ namespace collision { class CollisionObject; class FCLCollisionGeometryUserData; -class FCLCollisionObjectData : public CollisionObjectData +class FCLCollisionObjectEngineData : public CollisionObjectEngineData { public: /// Constructor - FCLCollisionObjectData(CollisionObject* parent, + FCLCollisionObjectEngineData(CollisionObject* parent, const dynamics::ShapePtr& shape); // Documentation inherited @@ -78,4 +78,4 @@ class FCLCollisionObjectData : public CollisionObjectData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECTENGINEDATA_H_ diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index 68a4a56689b52..6bf639e55a8a7 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -36,28 +36,16 @@ #include "dart/collision/fcl/FCLEngine.h" -#include - +#include #include #include #include -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Skeleton.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionGroup.h" -#include "dart/collision/fcl/FCLCollisionNode.h" -#include "dart/collision/fcl/FCLCollisionGroupData.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/MeshShape.h" -#include "dart/dynamics/SoftMeshShape.h" +#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/collision/fcl/FCLCollisionObjectEngineData.h" +#include "dart/collision/fcl/FCLCollisionGroupEngineData.h" namespace dart { namespace collision { @@ -68,7 +56,6 @@ namespace { // collision algorithm. struct CollisionData { - // Collision request fcl::CollisionRequest request; @@ -114,18 +101,18 @@ const std::string& FCLEngine::getTypeStatic() } //============================================================================== -CollisionObjectData* FCLEngine::createCollisionObjectData( +CollisionObjectEngineData* FCLEngine::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { - return new FCLCollisionObjectData(parent, shape); + return new FCLCollisionObjectEngineData(parent, shape); } //============================================================================== -CollisionGroupData* FCLEngine::createCollisionGroupData( +CollisionGroupEngineData* FCLEngine::createCollisionGroupData( std::vector collObjects) { - return new FCLCollisionGroupData(collObjects); + return new FCLCollisionGroupEngineData(collObjects); } //============================================================================== @@ -134,7 +121,7 @@ bool FCLEngine::detect(CollisionObject* object1, const Option& option, Result& result) { - result.clear(); + result.contacts.clear(); assert(object1->getEngine()->getType() == FCLEngine::getTypeStatic()); assert(object2->getEngine()->getType() == FCLEngine::getTypeStatic()); @@ -142,8 +129,8 @@ bool FCLEngine::detect(CollisionObject* object1, object1->updateEngineData(); object2->updateEngineData(); - auto data1 = static_cast(object1->getEngineData()); - auto data2 = static_cast(object2->getEngineData()); + auto data1 = static_cast(object1->getEngineData()); + auto data2 = static_cast(object2->getEngineData()); auto fclCollObj1 = data1->getFCLCollisionObject(); auto fclCollObj2 = data2->getFCLCollisionObject(); @@ -155,21 +142,51 @@ bool FCLEngine::detect(CollisionObject* object1, convert(collData.result, result); - return !result.empty(); + return !result.contacts.empty(); +} + +//============================================================================== +bool FCLEngine::detect(CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result) +{ + result.contacts.clear(); + + assert(object); + assert(group); + assert(object->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(group->getEngine()->getType() == FCLEngine::getTypeStatic()); + + object->updateEngineData(); + group->updateEngineData(); + + auto objData = static_cast(object->getEngineData()); + auto groupData = static_cast(group->getEngineData()); + + auto fclObject = objData->getFCLCollisionObject(); + auto broadPhaseAlg = groupData->getFCLCollisionManager(); + + CollisionData collData; + convert(option, collData.request); + + broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); + + convert(collData.result, result); + + return !result.contacts.empty(); } //============================================================================== bool FCLEngine::detect(CollisionGroup* group, const Option& option, Result& result) { - result.clear(); + result.contacts.clear(); assert(group); assert(group->getEngine()->getType() == FCLEngine::getTypeStatic()); group->updateEngineData(); - auto data = static_cast(group->getEngineData()); + auto data = static_cast(group->getEngineData()); auto broadPhaseAlg = data->getFCLCollisionManager(); @@ -180,14 +197,14 @@ bool FCLEngine::detect(CollisionGroup* group, convert(collData.result, result); - return !result.empty(); + return !result.contacts.empty(); } //============================================================================== bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { - result.clear(); + result.contacts.clear(); assert(group1); assert(group2); @@ -197,8 +214,8 @@ bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, group1->updateEngineData(); group2->updateEngineData(); - auto data1 = static_cast(group1->getEngineData()); - auto data2 = static_cast(group2->getEngineData()); + auto data1 = static_cast(group1->getEngineData()); + auto data2 = static_cast(group2->getEngineData()); auto broadPhaseAlg1 = data1->getFCLCollisionManager(); auto broadPhaseAlg2 = data2->getFCLCollisionManager(); @@ -210,34 +227,9 @@ bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, convert(collData.result, result); - return !result.empty(); + return !result.contacts.empty(); } -//============================================================================== -//bool FCLEngine::detect(CollisionGroup* group, -// const Option& option, Result& result) -//{ -// result.clear(); - -// assert(group->getEngineType() == FCL); - -// auto data = static_cast(group->getEngineData()); - -// auto broadPhaseAlg = data->getFCLBroadPhaseAlg(); - -//// object1->updateEngineData(); -//// object2->updateEngineData(); - -// CollisionData collData; -// convert(option, collData.request); - -//// collisionCallBack(fclCollObj1, fclCollObj2, &collData); - -// convert(collData.result, result); - -// return !result.empty(); -//} - namespace { diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index 6d8faee92a1b4..aa9fc0b694ec2 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -37,6 +37,8 @@ #ifndef DART_COLLISION_FCL_FCLENGINE_H_ #define DART_COLLISION_FCL_FCLENGINE_H_ +#include + #include "dart/collision/Engine.h" namespace dart { @@ -56,18 +58,22 @@ class FCLEngine : public Engine const std::string& getType() const override; // Documentation inherit - CollisionObjectData* createCollisionObjectData( + CollisionObjectEngineData* createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; // Documentation inherit - CollisionGroupData* createCollisionGroupData( + CollisionGroupEngineData* createCollisionGroupData( std::vector collObjects) override; // Documentation inherit bool detect(CollisionObject* object1, CollisionObject* object2, const Option& option, Result& result) override; + // Documentation inherit + bool detect(CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result) override; + // Documentation inherit bool detect(CollisionGroup* group, const Option& option, Result& result) override; diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 2e52ac172e203..c5b2600fbb3e6 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -541,6 +541,49 @@ TEST_F(COLLISION, FreeCollisionObject) } } +//============================================================================== +TEST_F(COLLISION, FreeCollisionGroupObject) +{ + collision::FCLEngine fclEngine; + + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + + collision::FreeCollisionObject obj1(&fclEngine, shape1); + collision::FreeCollisionObject obj2(&fclEngine, shape2); + collision::FreeCollisionObject obj3(&fclEngine, shape3); + + collision::CollisionGroup group(&fclEngine); + group.addCollisionObject(&obj1); + group.addCollisionObject(&obj2); + + obj1.setTranslation(Eigen::Vector3d::Zero()); + obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + obj3.setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); + + collision::Option option; + collision::Result result; + + EXPECT_FALSE(obj3.detect(&group, option, result)); + + obj3.setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); + EXPECT_TRUE(obj3.detect(&group, option, result)); + + EXPECT_EQ(result.contacts.size(), 4u); + + for (auto contact : result.contacts) + { + auto freeCollObj1 = dynamic_cast( + contact.collisionObject1); + auto freeCollObj2 = dynamic_cast( + contact.collisionObject2); + + EXPECT_NE(freeCollObj1, nullptr); + EXPECT_NE(freeCollObj2, nullptr); + } +} + //============================================================================== TEST_F(COLLISION, FreeCollisionGroup) { From 054fc149b4efd994e29779d34e929a90f8569d18 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 16 Feb 2016 07:50:58 -0500 Subject: [PATCH 04/67] Change owership management of collision objects -- still need more work --- dart/collision/CollisionDetector.h | 5 +- dart/collision/CollisionGroup.cpp | 25 ++- dart/collision/CollisionGroup.h | 22 ++- dart/collision/CollisionObject.cpp | 8 +- dart/collision/CollisionObject.h | 14 +- dart/collision/Engine.h | 14 +- dart/collision/SmartPointer.h | 58 ++++++ dart/collision/fcl/FCLCollisionDetector.cpp | 2 +- .../fcl/FCLCollisionGroupEngineData.cpp | 3 +- .../fcl/FCLCollisionGroupEngineData.h | 6 +- dart/collision/fcl/FCLCollisionNode.cpp | 14 +- dart/collision/fcl/FCLCollisionNode.h | 19 +- .../fcl/FCLCollisionObjectEngineData.h | 4 +- dart/collision/fcl/FCLEngine.cpp | 24 ++- dart/collision/fcl/FCLEngine.h | 8 +- dart/dynamics/CollisionDetector.cpp | 94 +++++++++ dart/dynamics/CollisionDetector.h | 166 ++++++++++++++++ unittests/testCollision.cpp | 187 +++++++----------- 18 files changed, 488 insertions(+), 185 deletions(-) create mode 100644 dart/collision/SmartPointer.h create mode 100644 dart/dynamics/CollisionDetector.cpp create mode 100644 dart/dynamics/CollisionDetector.h diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 0809e63a017dc..017c5368ba04e 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -80,9 +80,8 @@ struct Contact { /// Second colliding shape of the first body node dynamics::ShapePtr shape2; - collision::CollisionObject* collisionObject1; - collision::CollisionObject* collisionObject2; - // TODO(JS): shared_ptr + CollisionObject* collisionObject1; + CollisionObject* collisionObject2; /// Penetration depth double penetrationDepth; diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 23cd8530f8cc6..4123d6e8f7c52 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -46,15 +46,24 @@ namespace collision { //============================================================================== CollisionGroup::CollisionGroup( - Engine* engine, + const EnginePtr engine, const CollisionGroup::CollisionObjects& collObjects) - : mEngine(engine) + : mEngine(engine), + mCollisionObjects(collObjects), + mEngineData(mEngine->createCollisionGroupData(mCollisionObjects)) { assert(mEngine); +} - mCollisionObjects = collObjects; - - mEngineData.reset(mEngine->createCollisionGroupData(mCollisionObjects)); +//============================================================================== +CollisionGroup::CollisionGroup( + const EnginePtr& engine, + const CollisionObjectPtr& collObject) + : mEngine(engine), + mCollisionObjects{collObject} +{ + assert(mEngine); + assert(collObject); } //============================================================================== @@ -65,11 +74,11 @@ CollisionGroup::~CollisionGroup() //============================================================================== Engine* CollisionGroup::getEngine() const { - return mEngine; + return mEngine.get(); } //============================================================================== -void CollisionGroup::addCollisionObject(CollisionObject* object) +void CollisionGroup::addCollisionObject(const CollisionObjectPtr& object) { auto found = std::find(mCollisionObjects.begin(), mCollisionObjects.end(), object) @@ -82,7 +91,7 @@ void CollisionGroup::addCollisionObject(CollisionObject* object) } mCollisionObjects.push_back(object); - mEngineData->notifyCollisionObjectAdded(object); + mEngineData->notifyCollisionObjectAdded(object.get()); } //============================================================================== diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 86d3effeedbd6..c67441dd935a0 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -52,23 +52,25 @@ class CollisionGroup { public: - using CollisionObjects = std::vector; + using CollisionObjectPtr = std::shared_ptr; + using CollisionObjects = std::vector; - /// Default constructor - CollisionGroup( - Engine* engine, - const CollisionObjects& collObjects = CollisionObjects()); + /// Constructor + CollisionGroup(const EnginePtr engine, + const CollisionObjects& collObjects = CollisionObjects()); - /// Default destructor + /// Constructor + CollisionGroup(const EnginePtr& engine, + const CollisionObjectPtr& collObject); + + /// Destructor virtual ~CollisionGroup(); /// Return collision detection engine associated with this CollisionGroup Engine* getEngine() const; /// Add collision object to this CollisionGroup - void addCollisionObject(CollisionObject* object); - -// virtual void addCollisionNode(const CollisionNodes& nodes); + void addCollisionObject(const CollisionObjectPtr& object); /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); @@ -96,7 +98,7 @@ class CollisionGroup protected: /// Collision engine - Engine* mEngine; + EnginePtr mEngine; /// Collision objects CollisionObjects mCollisionObjects; diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index e126993adb722..274ab542a24f1 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -44,7 +44,7 @@ namespace collision { //============================================================================== Engine* CollisionObject::getEngine() const { - return mEngine; + return mEngine.get(); } //============================================================================== @@ -81,7 +81,7 @@ void CollisionObject::updateEngineData() } //============================================================================== -CollisionObject::CollisionObject(Engine* engine, +CollisionObject::CollisionObject(const EnginePtr& engine, const dynamics::ShapePtr& shape) : mEngine(engine), mShape(shape) @@ -89,11 +89,11 @@ CollisionObject::CollisionObject(Engine* engine, assert(mEngine); assert(mShape); - mEngineData.reset(mEngine->createCollisionObjectData(this, mShape)); + mEngineData = mEngine->createCollisionObjectData(this, mShape); } //============================================================================== -FreeCollisionObject::FreeCollisionObject(Engine* engine, +FreeCollisionObject::FreeCollisionObject(const EnginePtr& engine, const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf) : CollisionObject(engine, shape), diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index a45b703bcc395..30a2a1ddad328 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -40,6 +40,7 @@ #include #include "dart/collision/Engine.h" +#include "dart/collision/SmartPointer.h" namespace dart { namespace collision { @@ -83,17 +84,17 @@ class CollisionObject protected: /// Contructor - CollisionObject(Engine* engine, const dynamics::ShapePtr& shape); + CollisionObject(const EnginePtr& engine, const dynamics::ShapePtr& shape); protected: - /// Collision engine - Engine* mEngine; + /// Collision detection engine + EnginePtr mEngine; /// Shape dynamics::ShapePtr mShape; - /// Engine specific data + /// Collision detection engine specific data std::shared_ptr mEngineData; }; @@ -105,8 +106,7 @@ class FreeCollisionObject : public CollisionObject public: /// Constructor - FreeCollisionObject( - Engine* engine, + FreeCollisionObject(const EnginePtr& engine, const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); // TODO(JS): change to engine pointer @@ -130,8 +130,6 @@ class FreeCollisionObject : public CollisionObject }; -using CollisionObjectPtr = std::shared_ptr; - } // namespace collision } // namespace dart diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h index c44cdd21ffa20..988dcffe4ad40 100644 --- a/dart/collision/Engine.h +++ b/dart/collision/Engine.h @@ -40,16 +40,12 @@ #include #include "dart/collision/CollisionDetector.h" +#include "dart/collision/SmartPointer.h" #include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { -class CollisionGroup; -class CollisionGroupEngineData; -class CollisionObject; -class CollisionObjectEngineData; - struct Option { /// Flag whether compute contact information such as point, normal, and @@ -75,18 +71,20 @@ class Engine { public: + using CollisionObjects = std::vector; + /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; /// Create collision detection engine specific data for CollisionObject - virtual CollisionObjectEngineData* createCollisionObjectData( + virtual CollisionObjectEngineDataPtr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) = 0; // TODO(JS): shared_ptr /// Create collision detection engine specific data for CollisionGroup - virtual CollisionGroupEngineData* createCollisionGroupData( - std::vector collObjects) = 0; + virtual CollisionGroupEngineDataPtr createCollisionGroupData( + const CollisionObjects& collObjects) = 0; // TODO(JS): shared_ptr /// Perform collision detection for object1-object2. diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h new file mode 100644 index 0000000000000..4bbbea470b9b6 --- /dev/null +++ b/dart/collision/SmartPointer.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_SMARTPOINTER_H_ +#define DART_COLLISION_SMARTPOINTER_H_ + +#include "dart/common/SmartPointer.h" + +namespace dart { +namespace collision { + +DART_COMMON_MAKE_SHARED_WEAK(Engine) +DART_COMMON_MAKE_SHARED_WEAK(FCLEngine) + +DART_COMMON_MAKE_SHARED_WEAK(CollisionObject) +DART_COMMON_MAKE_SHARED_WEAK(CollisionObjectEngineData) +DART_COMMON_MAKE_SHARED_WEAK(FreeCollisionObject) + +DART_COMMON_MAKE_SHARED_WEAK(CollisionGroup) +DART_COMMON_MAKE_SHARED_WEAK(CollisionGroupEngineData) + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_SMARTPOINTER_H_ diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 452adc1fb7881..b709d69ec8188 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -258,7 +258,7 @@ FCLCollisionNode* FCLCollisionDetector::findCollisionNode( const fcl::CollisionObject* _fclCollObj) const { FCLCollisionGeometryUserData* userData = static_cast(_fclCollObj->getUserData()); - return userData->fclCollNode; + return userData->mFclCollNode; } } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionGroupEngineData.cpp b/dart/collision/fcl/FCLCollisionGroupEngineData.cpp index 353f8d0eabcfa..355e15be75b08 100644 --- a/dart/collision/fcl/FCLCollisionGroupEngineData.cpp +++ b/dart/collision/fcl/FCLCollisionGroupEngineData.cpp @@ -63,7 +63,8 @@ void FCLCollisionGroupEngineData::update() } //============================================================================== -void FCLCollisionGroupEngineData::notifyCollisionObjectAdded(CollisionObject* object) +void FCLCollisionGroupEngineData::notifyCollisionObjectAdded( + CollisionObject* object) { auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); diff --git a/dart/collision/fcl/FCLCollisionGroupEngineData.h b/dart/collision/fcl/FCLCollisionGroupEngineData.h index 18ada46fc06de..fdcc3f8621e5e 100644 --- a/dart/collision/fcl/FCLCollisionGroupEngineData.h +++ b/dart/collision/fcl/FCLCollisionGroupEngineData.h @@ -58,17 +58,21 @@ class FCLCollisionGroupEngineData : public CollisionGroupEngineData /// Constructor FCLCollisionGroupEngineData(const CollisionObjects& collObjects); + // Documentation inherited void update() override; + // Documentation inherited void notifyCollisionObjectAdded(CollisionObject* object) override; + // Documentation inherited void notifyCollisionObjectRemoved(CollisionObject* object) override; + /// Return FCL collision manager that is also a broad-phase algorithm FCLCollisionManager* getFCLCollisionManager() const; protected: - /// Broad-phase collision checker of FCL + /// FCL broad-phase algorithm std::unique_ptr mBroadPhaseAlg; }; diff --git a/dart/collision/fcl/FCLCollisionNode.cpp b/dart/collision/fcl/FCLCollisionNode.cpp index e148e16565f91..4e6587fcb370a 100644 --- a/dart/collision/fcl/FCLCollisionNode.cpp +++ b/dart/collision/fcl/FCLCollisionNode.cpp @@ -472,9 +472,9 @@ FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( FCLCollisionNode* fclCollNode, dynamics::BodyNode* bodyNode, const dynamics::ShapePtr& shape) - : fclCollNode(fclCollNode), - bodyNode(bodyNode), - shape(shape) + : mFclCollNode(fclCollNode), + mBodyNode(bodyNode), + mShape(shape) { // Do nothing } @@ -483,8 +483,8 @@ FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( CollisionObject* collisionObject, const dynamics::ShapePtr& shape) - : collisionObject(collisionObject), - shape(shape) + : mCollisionObject(collisionObject), + mShape(shape) { // Do nothing } @@ -674,8 +674,8 @@ void FCLCollisionNode::updateFCLCollisionObjects() FCLCollisionGeometryUserData* userData = static_cast(fclCollObj->collisionGeometry()->getUserData()); - BodyNode* bodyNode = userData->bodyNode; - Shape* shape = userData->shape.get(); + BodyNode* bodyNode = userData->mBodyNode; + Shape* shape = userData->mShape.get(); // Update soft-body's vertices if (shape->getShapeType() == Shape::SOFT_MESH) diff --git a/dart/collision/fcl/FCLCollisionNode.h b/dart/collision/fcl/FCLCollisionNode.h index 2d6e191cd74ec..65247afe43c84 100644 --- a/dart/collision/fcl/FCLCollisionNode.h +++ b/dart/collision/fcl/FCLCollisionNode.h @@ -45,6 +45,7 @@ #include #include "dart/collision/CollisionNode.h" +#include "dart/collision/SmartPointer.h" #include "dart/dynamics/Shape.h" namespace dart { @@ -62,17 +63,17 @@ class CollisionObject; struct FCLCollisionGeometryUserData { - FCLCollisionNode* fclCollNode; - CollisionObject* collisionObject; - dynamics::BodyNode* bodyNode; - dynamics::ShapePtr shape; + FCLCollisionNode* mFclCollNode; + CollisionObject* mCollisionObject; + dynamics::BodyNode* mBodyNode; + dynamics::ShapePtr mShape; - FCLCollisionGeometryUserData(FCLCollisionNode* fclCollNode, - dynamics::BodyNode* bodyNode, - const dynamics::ShapePtr& shape); + FCLCollisionGeometryUserData(FCLCollisionNode* mFclCollNode, + dynamics::BodyNode* mBodyNode, + const dynamics::ShapePtr& mShape); - FCLCollisionGeometryUserData(CollisionObject* fclCollNode, - const dynamics::ShapePtr& shape); + FCLCollisionGeometryUserData(CollisionObject* mFclCollNode, + const dynamics::ShapePtr& mShape); }; diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.h b/dart/collision/fcl/FCLCollisionObjectEngineData.h index cb0cf4ca9391d..3ea3fbf53e933 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.h +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.h @@ -56,7 +56,7 @@ class FCLCollisionObjectEngineData : public CollisionObjectEngineData /// Constructor FCLCollisionObjectEngineData(CollisionObject* parent, - const dynamics::ShapePtr& shape); + const dynamics::ShapePtr& shape); // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; @@ -64,6 +64,7 @@ class FCLCollisionObjectEngineData : public CollisionObjectEngineData // Documentation inherited void updateShape(const dynamics::ShapePtr& shape) override; + /// Return FCL collision object fcl::CollisionObject* getFCLCollisionObject() const; protected: @@ -71,6 +72,7 @@ class FCLCollisionObjectEngineData : public CollisionObjectEngineData /// FCL collision object std::unique_ptr mFCLCollisionObject; + /// FCL collision geometry user data std::unique_ptr mFCLCollisionGeometryUserData; }; diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index 6bf639e55a8a7..fc73d1fbeeddd 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -93,6 +93,12 @@ const std::string& FCLEngine::getType() const return getTypeStatic(); } +//============================================================================== +FCLEnginePtr FCLEngine::create() +{ + return std::make_shared(); +} + //============================================================================== const std::string& FCLEngine::getTypeStatic() { @@ -101,18 +107,18 @@ const std::string& FCLEngine::getTypeStatic() } //============================================================================== -CollisionObjectEngineData* FCLEngine::createCollisionObjectData( +CollisionObjectEngineDataPtr FCLEngine::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { - return new FCLCollisionObjectEngineData(parent, shape); + return std::make_shared(parent, shape); } //============================================================================== -CollisionGroupEngineData* FCLEngine::createCollisionGroupData( - std::vector collObjects) +CollisionGroupEngineDataPtr FCLEngine::createCollisionGroupData( + const CollisionObjects& collObjects) { - return new FCLCollisionGroupEngineData(collObjects); + return std::make_shared(collObjects); } //============================================================================== @@ -313,10 +319,10 @@ void convert(const fcl::Contact& fclContact, Contact& contact) = static_cast(fclContact.o2->getUserData()); assert(userData1); assert(userData2); - contact.shape1 = userData1->shape; - contact.shape2 = userData2->shape; - contact.collisionObject1 = userData1->collisionObject; - contact.collisionObject2 = userData2->collisionObject; + contact.shape1 = userData1->mShape; + contact.shape2 = userData2->mShape; + contact.collisionObject1 = userData1->mCollisionObject; + contact.collisionObject2 = userData2->mCollisionObject; } } // anonymous namespace diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index aa9fc0b694ec2..75203cea6754c 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -51,6 +51,8 @@ class FCLEngine : public Engine { public: + static FCLEnginePtr create(); + /// Return engine type "FCL" static const std::string& getTypeStatic(); @@ -58,13 +60,13 @@ class FCLEngine : public Engine const std::string& getType() const override; // Documentation inherit - CollisionObjectEngineData* createCollisionObjectData( + CollisionObjectEngineDataPtr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; // Documentation inherit - CollisionGroupEngineData* createCollisionGroupData( - std::vector collObjects) override; + CollisionGroupEngineDataPtr createCollisionGroupData( + const CollisionObjects& collObjects) override; // Documentation inherit bool detect(CollisionObject* object1, CollisionObject* object2, diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp new file mode 100644 index 0000000000000..9d9862f4055cf --- /dev/null +++ b/dart/dynamics/CollisionDetector.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/CollisionDetector.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +ShapeNodeCollisionObject::ShapeNodeCollisionObject( + const collision::EnginePtr& engine, + const dynamics::ShapePtr& shape, + const dynamics::BodyNodePtr& bodyNode) + : CollisionObject(engine, shape), + mBodyNode(bodyNode) +{ + auto found = false; + auto numShapes = mBodyNode->getNumCollisionShapes(); + for (auto i = 0u; i < numShapes; ++i) + { + auto shapeIt = mBodyNode->getCollisionShape(i); + if (shape == shapeIt) + { + found = true; + break; + } + } + + if (!found) + { + dtwarn << "[ShapeNodeCollisionObject::constructor] Attempting to create " + << "ShapeNodeCollisionObject with invalid pair of Shape and " + << "BodyNode.\n"; + assert(false); + } +} + +//============================================================================== +const Eigen::Isometry3d ShapeNodeCollisionObject::getTransform() const +{ + return mBodyNode->getWorldTransform() * mShape->getLocalTransform(); +} + +//============================================================================== +BodyNodePtr ShapeNodeCollisionObject::getBodyNode() const +{ + return mBodyNode; +} + +//============================================================================== +std::shared_ptr +CollisionDetector::createCollisionNode( + const collision::EnginePtr engine, + const ShapePtr& shape, + const BodyNodePtr& bodyNode) +{ + return std::make_shared(engine, shape, bodyNode); +} + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h new file mode 100644 index 0000000000000..d5649d2802e6d --- /dev/null +++ b/dart/dynamics/CollisionDetector.h @@ -0,0 +1,166 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_COLLISIONDETECTOR_H_ +#define DART_DYNAMICS_COLLISIONDETECTOR_H_ + +#include + +#include "dart/collision/CollisionDetector.h" +#include "dart/dynamics/SmartPointer.h" +#include "dart/collision/Engine.h" +#include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionGroup.h" +#include "dart/dynamics/BodyNode.h" + +namespace dart { +namespace dynamics { + +class ShapeNodeCollisionObject : public collision::CollisionObject +{ +public: + + ShapeNodeCollisionObject(const collision::EnginePtr& engine, + const dynamics::ShapePtr& shape, + const dynamics::BodyNodePtr& bodyNode); + // TODO(JS): this should be replaced by ShapeNode + + // Documentation inherited + const Eigen::Isometry3d getTransform() const override; + + /// Return BodyNode pointer associated with this ShapeNodeCollisionObject + dynamics::BodyNodePtr getBodyNode() const; + +protected: + + dynamics::BodyNodePtr mBodyNode; + +}; + +class CollisionDetector +{ +public: + + static std::shared_ptr createCollisionNode( + const collision::EnginePtr engine, + const dynamics::ShapePtr& shape, + const BodyNodePtr& bodyNode); + + template + static bool detect(const dynamics::ShapePtr& shape1, + const BodyNodePtr& body1, + const dynamics::ShapePtr& shape2, + const BodyNodePtr& body2, + const collision::Option& option, + collision::Result& result); + + template + static bool detect(const dynamics::SkeletonPtr& skel1, + const dynamics::SkeletonPtr& skel2, + const collision::Option& option, + collision::Result& result); + + static std::vector + createShapeNodeCollisionObjects( + const collision::EnginePtr& engine, + const dynamics::SkeletonPtr& skel); + +}; + +//============================================================================== +template +bool CollisionDetector::detect( + const ShapePtr& shape1, const BodyNodePtr& body1, + const ShapePtr& shape2, const BodyNodePtr& body2, + const collision::Option& option, collision::Result& result) +{ + auto engine = ColDecEngine::create(); + + auto obj1 = ShapeNodeCollisionObject(engine, shape1, body1); + auto obj2 = ShapeNodeCollisionObject(engine, shape2, body2); + + return obj1.detect(&obj2, option, result); +} + +//============================================================================== +template +bool CollisionDetector::detect( + const dynamics::SkeletonPtr& skel1, + const dynamics::SkeletonPtr& skel2, + const collision::Option& option, collision::Result& result) +{ + auto engine = ColDecEngine::create(); + + std::vector objects1 + = createShapeNodeCollisionObjects(engine, skel1); + std::vector objects2 + = createShapeNodeCollisionObjects(engine, skel2); + + auto group1 = collision::CollisionGroup(engine, objects1); + auto group2 = collision::CollisionGroup(engine, objects2); + + return group1.detect(&group2, option, result); +} + +//============================================================================== +std::vector +CollisionDetector::createShapeNodeCollisionObjects( + const collision::EnginePtr& engine, + const dynamics::SkeletonPtr& skel) +{ + std::vector objects; + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + { + auto bodyNode = skel->getBodyNode(i); + auto numColShapes = bodyNode->getNumCollisionShapes(); + + for (auto j = 0u; j < numColShapes; ++j) + { + auto shape = bodyNode->getCollisionShape(j); + objects.push_back(createCollisionNode(engine, shape, bodyNode)); + } + } + + return objects; +} + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_COLLISIONDETECTOR_H_ diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index c5b2600fbb3e6..209d7bbc11547 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -44,11 +44,12 @@ #include "dart/config.h" #include "dart/common/common.h" #include "dart/math/math.h" -#include "dart/dynamics/dynamics.h" #include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLEngine.h" +#include "dart/dynamics/dynamics.h" #include "dart/simulation/simulation.h" #include "dart/utils/utils.h" +#include "TestHelpers.h" using namespace dart; using namespace common; @@ -506,28 +507,50 @@ TEST_F(COLLISION, FCL_BOX_BOX) //} //============================================================================== -TEST_F(COLLISION, FreeCollisionObject) +template +void testFreeCollisionObjects() { - collision::FCLEngine fclEngine; + auto engine = EngineType::create(); ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - collision::FreeCollisionObject obj1(&fclEngine, shape1); - collision::FreeCollisionObject obj2(&fclEngine, shape2); + collision::FreeCollisionObjectPtr obj1( + new collision::FreeCollisionObject(engine, shape1)); + collision::FreeCollisionObjectPtr obj2( + new collision::FreeCollisionObject(engine, shape2)); + collision::FreeCollisionObjectPtr obj3( + new collision::FreeCollisionObject(engine, shape3)); - obj1.setTranslation(Eigen::Vector3d::Zero()); - obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + collision::CollisionGroupPtr group1(new collision::CollisionGroup(engine)); + group1->addCollisionObject(obj1); + collision::CollisionGroupPtr group2(new collision::CollisionGroup(engine)); + group2->addCollisionObject(obj2); + group2->addCollisionObject(obj3); collision::Option option; collision::Result result; - EXPECT_FALSE(obj1.detect(&obj2, option, result)); - - obj2.setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); - EXPECT_TRUE(obj1.detect(&obj2, option, result)); - - EXPECT_EQ(result.contacts.size(), 4u); + obj1->setTranslation(Eigen::Vector3d::Zero()); + obj2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + obj3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); + EXPECT_FALSE(obj1->detect(obj2.get(), option, result)); + EXPECT_FALSE(obj2->detect(obj3.get(), option, result)); + EXPECT_FALSE(obj1->detect(group2.get(), option, result)); + EXPECT_FALSE(group1->detect(obj3.get(), option, result)); + EXPECT_FALSE(group1->detect(group2.get(), option, result)); + EXPECT_FALSE(group2->detect(option, result)); + + obj1->setTranslation(Eigen::Vector3d::Zero()); + obj2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); + obj3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); + EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); + EXPECT_TRUE(obj2->detect(obj3.get(), option, result)); + EXPECT_TRUE(obj1->detect(group2.get(), option, result)); + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(group2->detect(option, result)); + EXPECT_TRUE(group2->detect(option, result)); for (auto contact : result.contacts) { @@ -542,132 +565,72 @@ TEST_F(COLLISION, FreeCollisionObject) } //============================================================================== -TEST_F(COLLISION, FreeCollisionGroupObject) +TEST_F(COLLISION, FreeCollisionObjects) { - collision::FCLEngine fclEngine; - - ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - - collision::FreeCollisionObject obj1(&fclEngine, shape1); - collision::FreeCollisionObject obj2(&fclEngine, shape2); - collision::FreeCollisionObject obj3(&fclEngine, shape3); - - collision::CollisionGroup group(&fclEngine); - group.addCollisionObject(&obj1); - group.addCollisionObject(&obj2); - - obj1.setTranslation(Eigen::Vector3d::Zero()); - obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); - obj3.setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); - - collision::Option option; - collision::Result result; - - EXPECT_FALSE(obj3.detect(&group, option, result)); - - obj3.setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); - EXPECT_TRUE(obj3.detect(&group, option, result)); - - EXPECT_EQ(result.contacts.size(), 4u); - - for (auto contact : result.contacts) - { - auto freeCollObj1 = dynamic_cast( - contact.collisionObject1); - auto freeCollObj2 = dynamic_cast( - contact.collisionObject2); - - EXPECT_NE(freeCollObj1, nullptr); - EXPECT_NE(freeCollObj2, nullptr); - } + testFreeCollisionObjects(); } //============================================================================== -TEST_F(COLLISION, FreeCollisionGroup) +template +void testBodyNodes() { - collision::FCLEngine fclEngine; + auto engine = EngineType::create(); - ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + Eigen::Vector3d size(1.0, 1.0, 1.0); + Eigen::Vector3d pos1(0.0, 0.0, 0.0); + Eigen::Vector3d pos2(0.5, 0.0, 0.0); - collision::FreeCollisionObject obj1(&fclEngine, shape1); - collision::FreeCollisionObject obj2(&fclEngine, shape2); + auto boxSkel1 = createBox(size, pos1); + auto boxSkel2 = createBox(size, pos2); - collision::CollisionGroup group(&fclEngine); - group.addCollisionObject(&obj1); - group.addCollisionObject(&obj2); + auto boxBody1 = boxSkel1->getBodyNode(0u); + auto boxBody2 = boxSkel2->getBodyNode(0u); - obj1.setTranslation(Eigen::Vector3d::Zero()); - obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + auto boxShape1 = boxBody1->getCollisionShape(0u); + auto boxShape2 = boxBody2->getCollisionShape(0u); collision::Option option; collision::Result result; - EXPECT_FALSE(group.detect(option, result)); - - obj2.setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); - EXPECT_TRUE(group.detect(option, result)); - - EXPECT_EQ(result.contacts.size(), 4u); + auto hit = dynamics::CollisionDetector::detect(boxShape1, boxBody1, + boxShape2, boxBody2, + option, result); - for (auto contact : result.contacts) - { - auto freeCollObj1 = dynamic_cast( - contact.collisionObject1); - auto freeCollObj2 = dynamic_cast( - contact.collisionObject2); - - EXPECT_NE(freeCollObj1, nullptr); - EXPECT_NE(freeCollObj2, nullptr); - } + EXPECT_TRUE(hit); } //============================================================================== -TEST_F(COLLISION, FreeCollisionGroupGroup) +TEST_F(COLLISION, BodyNodeNodes) { - collision::FCLEngine fclEngine; - - ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - - collision::FreeCollisionObject obj1(&fclEngine, shape1); - collision::FreeCollisionObject obj2(&fclEngine, shape2); - collision::FreeCollisionObject obj3(&fclEngine, shape3); + testBodyNodes(); +} - collision::CollisionGroup group1(&fclEngine); - group1.addCollisionObject(&obj1); - group1.addCollisionObject(&obj2); +//============================================================================== +template +void testSkeletons() +{ + auto engine = EngineType::create(); - collision::CollisionGroup group2(&fclEngine); - group2.addCollisionObject(&obj3); + Eigen::Vector3d size(1.0, 1.0, 1.0); + Eigen::Vector3d pos1(0.0, 0.0, 0.0); + Eigen::Vector3d pos2(0.5, 0.0, 0.0); - obj1.setTranslation(Eigen::Vector3d::Zero()); - obj2.setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); - obj3.setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); + auto boxSkel1 = createBox(size, pos1); + auto boxSkel2 = createBox(size, pos2); collision::Option option; collision::Result result; - EXPECT_FALSE(group1.detect(&group2, option, result)); - - obj3.setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); - EXPECT_TRUE(group1.detect(&group2, option, result)); - - EXPECT_EQ(result.contacts.size(), 4u); + auto hit = dynamics::CollisionDetector::detect( + boxSkel1, boxSkel2, option, result); - for (auto contact : result.contacts) - { - auto freeCollObj1 = dynamic_cast( - contact.collisionObject1); - auto freeCollObj2 = dynamic_cast( - contact.collisionObject2); + EXPECT_TRUE(hit); +} - EXPECT_NE(freeCollObj1, nullptr); - EXPECT_NE(freeCollObj2, nullptr); - } +//============================================================================== +TEST_F(COLLISION, Skeletons) +{ + testSkeletons(); } //============================================================================== From 1d437c632ce6d5e9482e7dcc3cf2c28f6b7977eb Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 16 Feb 2016 11:17:13 -0500 Subject: [PATCH 05/67] Implement FCLMeshEngine --- dart/collision/CollisionGroup.h | 1 + dart/collision/CollisionObject.cpp | 2 +- dart/collision/CollisionObjectEngineData.h | 4 + dart/collision/SmartPointer.h | 5 + .../fcl/FCLCollisionObjectEngineData.cpp | 121 +++- .../fcl/FCLCollisionObjectEngineData.h | 9 +- dart/collision/fcl/FCLEngine.cpp | 2 +- dart/collision/fcl/FCLEngine.h | 6 +- .../FCLMeshCollisionGroupEngineData.cpp | 92 +++ .../FCLMeshCollisionGroupEngineData.h | 83 +++ .../FCLMeshCollisionObjectEngineData.cpp | 652 ++++++++++++++++++ .../FCLMeshCollisionObjectEngineData.h | 86 +++ dart/collision/fcl_mesh/FCLMeshEngine.cpp | 539 +++++++++++++++ dart/collision/fcl_mesh/FCLMeshEngine.h | 96 +++ dart/constraint/ConstraintSolver.cpp | 5 + dart/constraint/ConstraintSolver.h | 7 + unittests/testCollision.cpp | 4 + 17 files changed, 1683 insertions(+), 31 deletions(-) create mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp create mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h create mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp create mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h create mode 100644 dart/collision/fcl_mesh/FCLMeshEngine.cpp create mode 100644 dart/collision/fcl_mesh/FCLMeshEngine.h diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index c67441dd935a0..1f344b3449515 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -99,6 +99,7 @@ class CollisionGroup /// Collision engine EnginePtr mEngine; + // TODO(JS): Engine* ? /// Collision objects CollisionObjects mCollisionObjects; diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 274ab542a24f1..3f0e5686984e5 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -77,7 +77,7 @@ CollisionObjectEngineData* CollisionObject::getEngineData() const //============================================================================== void CollisionObject::updateEngineData() { - mEngineData->updateTransform(getTransform()); + mEngineData->update(); } //============================================================================== diff --git a/dart/collision/CollisionObjectEngineData.h b/dart/collision/CollisionObjectEngineData.h index 499be890686ce..3abe2a7833668 100644 --- a/dart/collision/CollisionObjectEngineData.h +++ b/dart/collision/CollisionObjectEngineData.h @@ -52,6 +52,10 @@ class CollisionObjectEngineData virtual void updateShape(const dynamics::ShapePtr& shape) = 0; + /// Update engine data. This function will be called ahead of every collision + /// checking + virtual void update() = 0; + }; } // namespace collision diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h index 4bbbea470b9b6..09db3d0c682a2 100644 --- a/dart/collision/SmartPointer.h +++ b/dart/collision/SmartPointer.h @@ -37,6 +37,7 @@ #ifndef DART_COLLISION_SMARTPOINTER_H_ #define DART_COLLISION_SMARTPOINTER_H_ +#include "dart/config.h" #include "dart/common/SmartPointer.h" namespace dart { @@ -44,6 +45,10 @@ namespace collision { DART_COMMON_MAKE_SHARED_WEAK(Engine) DART_COMMON_MAKE_SHARED_WEAK(FCLEngine) +DART_COMMON_MAKE_SHARED_WEAK(FCLMeshEngine) +#ifdef HAVE_BULLET_COLLISION + DART_COMMON_MAKE_SHARED_WEAK(BulletEngine) +#endif DART_COMMON_MAKE_SHARED_WEAK(CollisionObject) DART_COMMON_MAKE_SHARED_WEAK(CollisionObjectEngineData) diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.cpp b/dart/collision/fcl/FCLCollisionObjectEngineData.cpp index 6e3e18583f8f8..2e79332ff427f 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.cpp @@ -43,6 +43,7 @@ #include "dart/common/Console.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/Engine.h" +#include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLCollisionNode.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" @@ -58,6 +59,8 @@ namespace collision { namespace { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + //============================================================================== // Create a cube mesh for collision detection template @@ -308,6 +311,10 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) return model; } +#endif + +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) + //============================================================================== template fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, @@ -411,6 +418,8 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, return model; } +#endif + //============================================================================== template fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, @@ -467,28 +476,10 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) return model; } -} // anonymous namespace - -//============================================================================== -FCLCollisionObjectEngineData::FCLCollisionObjectEngineData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) - : CollisionObjectEngineData(), - mFCLCollisionGeometryUserData( - new FCLCollisionGeometryUserData(parent, shape)) -{ - updateShape(shape); -} - //============================================================================== -void FCLCollisionObjectEngineData::updateTransform(const Eigen::Isometry3d& tf) -{ - mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); - mFCLCollisionObject->computeAABB(); -} - -//============================================================================== -void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) +boost::shared_ptr createFCLCollisionGeometry( + const dynamics::ShapePtr& shape, + FCLCollisionGeometryUserData* userData) { using dynamics::Shape; using dynamics::BoxShape; @@ -594,19 +585,99 @@ void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) } default: { - dterr << "[FCLCollisionObjectData::createFCLCollisionObject] " + dterr << "[FCLCollisionObjectData::updateShape] " << "Attempting to create unsupported shape type '" << shape->getShapeType() << "'.\n"; - return; + return nullptr; } } - assert(fclCollGeom); - fclCollGeom->setUserData(mFCLCollisionGeometryUserData.get()); + fclCollGeom->setUserData(userData); + + return fclCollGeom; +} + +} // anonymous namespace + +//============================================================================== +FCLCollisionObjectEngineData::FCLCollisionObjectEngineData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) + : CollisionObjectEngineData(), + mFCLCollisionGeometryUserData( + new FCLCollisionGeometryUserData(parent, shape)), + mFCLCollisionObject( + new fcl::CollisionObject( + createFCLCollisionGeometry(shape, mFCLCollisionGeometryUserData.get()))) +{ + // Do nothing +} + +//============================================================================== +void FCLCollisionObjectEngineData::updateTransform(const Eigen::Isometry3d& tf) +{ + mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); + mFCLCollisionObject->computeAABB(); +} + +//============================================================================== +void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) +{ + auto fclCollGeom = createFCLCollisionGeometry( + shape, mFCLCollisionGeometryUserData.get()); mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); } +//============================================================================== +void FCLCollisionObjectEngineData::update() +{ + using dart::dynamics::BodyNode; + using dart::dynamics::Shape; + using dart::dynamics::SoftMeshShape; + + auto collisionObject = mFCLCollisionGeometryUserData->mCollisionObject; + auto shape = collisionObject->getShape().get(); + + if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) + { + // Update soft-body's vertices + if (shape->getShapeType() == Shape::SOFT_MESH) + { + assert(dynamic_cast(shape)); + SoftMeshShape* softMeshShape = static_cast(shape); + + const aiMesh* mesh = softMeshShape->getAssimpMesh(); + softMeshShape->update(); +#if FCL_VERSION_AT_LEAST(0,3,0) + auto collGeom = const_cast( + mFCLCollisionObject->collisionGeometry().get()); +#else + fcl::CollisionGeometry* collGeom + = const_cast( + mFCLCollisionObject->getCollisionGeometry()); +#endif + assert(dynamic_cast*>(collGeom)); + auto bvhModel = static_cast*>(collGeom); + + bvhModel->beginUpdateModel(); + for (auto j = 0u; j < mesh->mNumFaces; ++j) + { + fcl::Vec3f vertices[3]; + for (auto k = 0u; k < 3; ++k) + { + const auto& vertex = mesh->mVertices[mesh->mFaces[j].mIndices[k]]; + vertices[k] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + } + bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]); + } + bvhModel->endUpdateModel(); + } + } + + updateTransform(collisionObject->getTransform()); +} + //============================================================================== fcl::CollisionObject* FCLCollisionObjectEngineData::getFCLCollisionObject() const { diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.h b/dart/collision/fcl/FCLCollisionObjectEngineData.h index 3ea3fbf53e933..1d6a273a25241 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.h +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.h @@ -64,17 +64,20 @@ class FCLCollisionObjectEngineData : public CollisionObjectEngineData // Documentation inherited void updateShape(const dynamics::ShapePtr& shape) override; + // Documentation inherited + void update() override; + /// Return FCL collision object fcl::CollisionObject* getFCLCollisionObject() const; protected: - /// FCL collision object - std::unique_ptr mFCLCollisionObject; - /// FCL collision geometry user data std::unique_ptr mFCLCollisionGeometryUserData; + /// FCL collision object + std::unique_ptr mFCLCollisionObject; + }; } // namespace collision diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index fc73d1fbeeddd..23bc9aa94359e 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -96,7 +96,7 @@ const std::string& FCLEngine::getType() const //============================================================================== FCLEnginePtr FCLEngine::create() { - return std::make_shared(); + return FCLEnginePtr(new FCLEngine()); } //============================================================================== diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index 75203cea6754c..df92e1df69abb 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -46,7 +46,7 @@ namespace collision { class FCLCollisionGroup; -/// FCL Collidion detection engine +/// FCL Collision detection engine class FCLEngine : public Engine { public: @@ -84,6 +84,10 @@ class FCLEngine : public Engine bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) override; +protected: + + FCLEngine() = default; + }; } // namespace collision diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp new file mode 100644 index 0000000000000..c01693ecc3669 --- /dev/null +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h" + +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h" + +namespace dart { +namespace collision { + +//============================================================================== +FCLMeshCollisionGroupEngineData::FCLMeshCollisionGroupEngineData( + const FCLMeshCollisionGroupEngineData::CollisionObjects& collObjects) + : mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +{ + for (auto collObj : collObjects) + { + auto data = static_cast(collObj->getEngineData()); + mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + } + + mBroadPhaseAlg->setup(); +} + +//============================================================================== +void FCLMeshCollisionGroupEngineData::update() +{ + mBroadPhaseAlg->update(); +} + +//============================================================================== +void FCLMeshCollisionGroupEngineData::notifyCollisionObjectAdded( + CollisionObject* object) +{ + auto data = static_cast(object->getEngineData()); + mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + mBroadPhaseAlg->setup(); +} + +//============================================================================== +void FCLMeshCollisionGroupEngineData::notifyCollisionObjectRemoved( + CollisionObject* object) +{ + auto data = static_cast(object->getEngineData()); + mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); + mBroadPhaseAlg->setup(); +} + +//============================================================================== +FCLMeshCollisionGroupEngineData::FCLCollisionManager* +FCLMeshCollisionGroupEngineData::getFCLCollisionManager() const +{ + return mBroadPhaseAlg.get(); +} + + +} // namespace collision +} // namespace dart diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h new file mode 100644 index 0000000000000..81131a704d201 --- /dev/null +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPENGINEDATA_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPENGINEDATA_H_ + +#include + +#include "dart/collision/CollisionGroup.h" +#include "dart/collision/CollisionGroupEngineData.h" + +namespace dart { +namespace collision { + +class CollisionObject; +class FCLCollisionGeometryUserData; + +class FCLMeshCollisionGroupEngineData : public CollisionGroupEngineData +{ +public: + + using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; + using CollisionObjects = CollisionGroup::CollisionObjects; + + /// Constructor + FCLMeshCollisionGroupEngineData(const CollisionObjects& collObjects); + + // Documentation inherited + void update() override; + + // Documentation inherited + void notifyCollisionObjectAdded(CollisionObject* object) override; + + // Documentation inherited + void notifyCollisionObjectRemoved(CollisionObject* object) override; + + /// Return FCL collision manager that is also a broad-phase algorithm + FCLCollisionManager* getFCLCollisionManager() const; + +protected: + + /// FCL broad-phase algorithm + std::unique_ptr mBroadPhaseAlg; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPENGINEDATA_H_ diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp new file mode 100644 index 0000000000000..e8939163606c4 --- /dev/null +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp @@ -0,0 +1,652 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h" + +#include +#include +#include + +#include "dart/common/Console.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/Engine.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" + +namespace dart { +namespace collision { + +namespace { + +//============================================================================== +// Create a cube mesh for collision detection +template +fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) +{ + int faces[6][4] = + { + {0, 1, 2, 3}, + {3, 2, 6, 7}, + {7, 6, 5, 4}, + {4, 5, 1, 0}, + {5, 6, 2, 1}, + {7, 4, 0, 3} + }; + float v[8][3]; + + v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; + v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; + v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; + v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; + v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; + v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 6; i++) + { + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + model->addTriangle(p1, p2, p3); + + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + model->addTriangle(p1, p2, p3); + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) +{ + float v[59][3] = + { + {0, 0, 0}, + {0.135299, -0.461940, -0.135299}, + {0.000000, -0.461940, -0.191342}, + {-0.135299, -0.461940, -0.135299}, + {-0.191342, -0.461940, 0.000000}, + {-0.135299, -0.461940, 0.135299}, + {0.000000, -0.461940, 0.191342}, + {0.135299, -0.461940, 0.135299}, + {0.191342, -0.461940, 0.000000}, + {0.250000, -0.353553, -0.250000}, + {0.000000, -0.353553, -0.353553}, + {-0.250000, -0.353553, -0.250000}, + {-0.353553, -0.353553, 0.000000}, + {-0.250000, -0.353553, 0.250000}, + {0.000000, -0.353553, 0.353553}, + {0.250000, -0.353553, 0.250000}, + {0.353553, -0.353553, 0.000000}, + {0.326641, -0.191342, -0.326641}, + {0.000000, -0.191342, -0.461940}, + {-0.326641, -0.191342, -0.326641}, + {-0.461940, -0.191342, 0.000000}, + {-0.326641, -0.191342, 0.326641}, + {0.000000, -0.191342, 0.461940}, + {0.326641, -0.191342, 0.326641}, + {0.461940, -0.191342, 0.000000}, + {0.353553, 0.000000, -0.353553}, + {0.000000, 0.000000, -0.500000}, + {-0.353553, 0.000000, -0.353553}, + {-0.500000, 0.000000, 0.000000}, + {-0.353553, 0.000000, 0.353553}, + {0.000000, 0.000000, 0.500000}, + {0.353553, 0.000000, 0.353553}, + {0.500000, 0.000000, 0.000000}, + {0.326641, 0.191342, -0.326641}, + {0.000000, 0.191342, -0.461940}, + {-0.326641, 0.191342, -0.326641}, + {-0.461940, 0.191342, 0.000000}, + {-0.326641, 0.191342, 0.326641}, + {0.000000, 0.191342, 0.461940}, + {0.326641, 0.191342, 0.326641}, + {0.461940, 0.191342, 0.000000}, + {0.250000, 0.353553, -0.250000}, + {0.000000, 0.353553, -0.353553}, + {-0.250000, 0.353553, -0.250000}, + {-0.353553, 0.353553, 0.000000}, + {-0.250000, 0.353553, 0.250000}, + {0.000000, 0.353553, 0.353553}, + {0.250000, 0.353553, 0.250000}, + {0.353553, 0.353553, 0.000000}, + {0.135299, 0.461940, -0.135299}, + {0.000000, 0.461940, -0.191342}, + {-0.135299, 0.461940, -0.135299}, + {-0.191342, 0.461940, 0.000000}, + {-0.135299, 0.461940, 0.135299}, + {0.000000, 0.461940, 0.191342}, + {0.135299, 0.461940, 0.135299}, + {0.191342, 0.461940, 0.000000}, + {0.000000, -0.500000, 0.000000}, + {0.000000, 0.500000, 0.000000} + }; + + int f[112][3] = + { + {1, 2, 9}, + {9, 2, 10}, + {2, 3, 10}, + {10, 3, 11}, + {3, 4, 11}, + {11, 4, 12}, + {4, 5, 12}, + {12, 5, 13}, + {5, 6, 13}, + {13, 6, 14}, + {6, 7, 14}, + {14, 7, 15}, + {7, 8, 15}, + {15, 8, 16}, + {8, 1, 16}, + {16, 1, 9}, + {9, 10, 17}, + {17, 10, 18}, + {10, 11, 18}, + {18, 11, 19}, + {11, 12, 19}, + {19, 12, 20}, + {12, 13, 20}, + {20, 13, 21}, + {13, 14, 21}, + {21, 14, 22}, + {14, 15, 22}, + {22, 15, 23}, + {15, 16, 23}, + {23, 16, 24}, + {16, 9, 24}, + {24, 9, 17}, + {17, 18, 25}, + {25, 18, 26}, + {18, 19, 26}, + {26, 19, 27}, + {19, 20, 27}, + {27, 20, 28}, + {20, 21, 28}, + {28, 21, 29}, + {21, 22, 29}, + {29, 22, 30}, + {22, 23, 30}, + {30, 23, 31}, + {23, 24, 31}, + {31, 24, 32}, + {24, 17, 32}, + {32, 17, 25}, + {25, 26, 33}, + {33, 26, 34}, + {26, 27, 34}, + {34, 27, 35}, + {27, 28, 35}, + {35, 28, 36}, + {28, 29, 36}, + {36, 29, 37}, + {29, 30, 37}, + {37, 30, 38}, + {30, 31, 38}, + {38, 31, 39}, + {31, 32, 39}, + {39, 32, 40}, + {32, 25, 40}, + {40, 25, 33}, + {33, 34, 41}, + {41, 34, 42}, + {34, 35, 42}, + {42, 35, 43}, + {35, 36, 43}, + {43, 36, 44}, + {36, 37, 44}, + {44, 37, 45}, + {37, 38, 45}, + {45, 38, 46}, + {38, 39, 46}, + {46, 39, 47}, + {39, 40, 47}, + {47, 40, 48}, + {40, 33, 48}, + {48, 33, 41}, + {41, 42, 49}, + {49, 42, 50}, + {42, 43, 50}, + {50, 43, 51}, + {43, 44, 51}, + {51, 44, 52}, + {44, 45, 52}, + {52, 45, 53}, + {45, 46, 53}, + {53, 46, 54}, + {46, 47, 54}, + {54, 47, 55}, + {47, 48, 55}, + {55, 48, 56}, + {48, 41, 56}, + {56, 41, 49}, + {2, 1, 57}, + {3, 2, 57}, + {4, 3, 57}, + {5, 4, 57}, + {6, 5, 57}, + {7, 6, 57}, + {8, 7, 57}, + {1, 8, 57}, + {49, 50, 58}, + {50, 51, 58}, + {51, 52, 58}, + {52, 53, 58}, + {53, 54, 58}, + {54, 55, 58}, + {55, 56, 58}, + {56, 49, 58} + }; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 112; i++) + { + p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, + v[f[i][0]][1] * _sizeY, + v[f[i][0]][2] * _sizeZ); + p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, + v[f[i][1]][1] * _sizeY, + v[f[i][1]][2] * _sizeZ); + p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, + v[f[i][2]][1] * _sizeY, + v[f[i][2]][2] * _sizeZ); + + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + + return model; +} + +//============================================================================== +template +fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, + double _height, int _slices, int _stacks) +{ + const int CACHE_SIZE = 240; + + int i, j; + float sinCache[CACHE_SIZE]; + float cosCache[CACHE_SIZE]; + float angle; + float zBase; + float zLow, zHigh; + float sintemp, costemp; + float deltaRadius; + float radiusLow, radiusHigh; + + if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; + + if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || + _height < 0.0) + { + return nullptr; + } + + /* Center at CoM */ + zBase = -_height/2; + + /* Compute delta */ + deltaRadius = _baseRadius - _topRadius; + + /* Cache is the vertex locations cache */ + for (i = 0; i < _slices; i++) + { + angle = 2 * M_PI * i / _slices; + sinCache[i] = sin(angle); + cosCache[i] = cos(angle); + } + + sinCache[_slices] = sinCache[0]; + cosCache[_slices] = cosCache[0]; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3, p4; + + model->beginModel(); + + /* Base of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _baseRadius; + zLow = zBase; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + /* Body of cylinder */ + for (i = 0; i < _slices; i++) + { + for (j = 0; j < _stacks; j++) + { + zLow = j * _height / _stacks + zBase; + zHigh = (j + 1) * _height / _stacks + zBase; + radiusLow = _baseRadius + - deltaRadius * (static_cast(j) / _stacks); + radiusHigh = _baseRadius + - deltaRadius * (static_cast(j + 1) / _stacks); + + p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); + + model->addTriangle(p1, p2, p3); + model->addTriangle(p2, p3, p4); + } + } + + /* Top of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _topRadius; + zLow = zBase + _height; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, + const aiScene* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + for (size_t i = 0; i < _mesh->mNumMeshes; i++) + { + for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) + { + fcl::Vec3f vertices[3]; + for (size_t k = 0; k < 3; k++) + { + const aiVector3D& vertex + = _mesh->mMeshes[i]->mVertices[ + _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + vertices[k] = fcl::Vec3f(vertex.x * _scaleX, + vertex.y * _scaleY, + vertex.z * _scaleZ); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + + for (size_t i = 0; i < _mesh->mNumFaces; i++) + { + fcl::Vec3f vertices[3]; + for (size_t j = 0; j < 3; j++) + { + const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; + vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + model->endModel(); + return model; +} + +//============================================================================== +boost::shared_ptr createFCLCollisionGeometry( + const dynamics::ShapePtr& shape, + FCLCollisionGeometryUserData* userData) +{ + using ::dart::dynamics::Shape; + using ::dart::dynamics::BoxShape; + using ::dart::dynamics::EllipsoidShape; + using ::dart::dynamics::CylinderShape; + using ::dart::dynamics::PlaneShape; + using ::dart::dynamics::MeshShape; + using ::dart::dynamics::SoftMeshShape; + + boost::shared_ptr fclCollGeom; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + const BoxShape* box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); + fclCollGeom.reset(createCube(size[0], size[1], size[2])); + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + EllipsoidShape* ellipsoid = static_cast(shape.get()); + + const Eigen::Vector3d& size = ellipsoid->getSize(); + + fclCollGeom.reset( + createEllipsoid(size[0], size[1], size[2])); + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + const CylinderShape* cylinder + = static_cast(shape.get()); + const double radius = cylinder->getRadius(); + const double height = cylinder->getHeight(); + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); + // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 + // returns single contact point for cylinder yet. + break; + } + case Shape::PLANE: + { + fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + MeshShape* shapeMesh = static_cast(shape.get()); + fclCollGeom.reset(createMesh(shapeMesh->getScale()[0], + shapeMesh->getScale()[1], + shapeMesh->getScale()[2], + shapeMesh->getMesh())); + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + SoftMeshShape* softMeshShape = static_cast(shape.get()); + fclCollGeom.reset( + createSoftMesh(softMeshShape->getAssimpMesh())); + + break; + } + default: + { + dterr << "[FCLMeshCollisionObjectData::updateShape] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + return nullptr; + } + } + + fclCollGeom->setUserData(userData); + + return fclCollGeom; +} + +} // anonymous namespace + +//============================================================================== +FCLMeshCollisionObjectEngineData::FCLMeshCollisionObjectEngineData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) + : CollisionObjectEngineData(), + mFCLCollisionGeometryUserData( + new FCLCollisionGeometryUserData(parent, shape)), + mFCLCollisionObject( + new fcl::CollisionObject( + createFCLCollisionGeometry(shape, mFCLCollisionGeometryUserData.get()))) +{ + // Do nothing +} + +//============================================================================== +void FCLMeshCollisionObjectEngineData::updateTransform( + const Eigen::Isometry3d& tf) +{ + mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); + mFCLCollisionObject->computeAABB(); +} + +//============================================================================== +void FCLMeshCollisionObjectEngineData::updateShape( + const dynamics::ShapePtr& shape) +{ + auto fclCollGeom = createFCLCollisionGeometry( + shape, mFCLCollisionGeometryUserData.get()); + + mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); + mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); +} + +//============================================================================== +void FCLMeshCollisionObjectEngineData::update() +{ + using dart::dynamics::BodyNode; + using dart::dynamics::Shape; + using dart::dynamics::SoftMeshShape; + + auto collisionObject = mFCLCollisionGeometryUserData->mCollisionObject; + auto shape = collisionObject->getShape().get(); + + if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) + { + // Update soft-body's vertices + if (shape->getShapeType() == Shape::SOFT_MESH) + { + assert(dynamic_cast(shape)); + SoftMeshShape* softMeshShape = static_cast(shape); + const aiMesh* mesh = softMeshShape->getAssimpMesh(); + softMeshShape->update(); + +#if FCL_VERSION_AT_LEAST(0,3,0) + auto collGeom = const_cast( + mFCLCollisionObject->collisionGeometry().get()); +#else + fcl::CollisionGeometry* collGeom + = const_cast( + mFCLCollisionObject->getCollisionGeometry()); +#endif + + assert(dynamic_cast*>(collGeom)); + auto bvhModel = static_cast*>(collGeom); + + bvhModel->beginUpdateModel(); + + for (auto j = 0u; j < mesh->mNumFaces; ++j) + { + fcl::Vec3f vertices[3]; + for (auto k = 0u; k < 3; ++k) + { + const auto& vertex = mesh->mVertices[mesh->mFaces[j].mIndices[k]]; + vertices[k] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + } + bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]); + } + bvhModel->endUpdateModel(); + } + } + + updateTransform(collisionObject->getTransform()); +} + +//============================================================================== +fcl::CollisionObject* FCLMeshCollisionObjectEngineData::getFCLCollisionObject() const +{ + return mFCLCollisionObject.get(); +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h new file mode 100644 index 0000000000000..841c451f785d0 --- /dev/null +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h @@ -0,0 +1,86 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTENGINEDATA_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTENGINEDATA_H_ + +#include +#include + +#include + +#include "dart/collision/CollisionObjectEngineData.h" + +namespace dart { +namespace collision { + +class CollisionObject; +class FCLCollisionGeometryUserData; + +class FCLMeshCollisionObjectEngineData : public CollisionObjectEngineData +{ +public: + + /// Constructor + FCLMeshCollisionObjectEngineData(CollisionObject* parent, + const dynamics::ShapePtr& shape); + + // Documentation inherited + void updateTransform(const Eigen::Isometry3d& tf) override; + + // Documentation inherited + void updateShape(const dynamics::ShapePtr& shape) override; + + // Documentation inherited + void update() override; + + /// Return FCL collision object + fcl::CollisionObject* getFCLCollisionObject() const; + +protected: + + /// FCL collision geometry user data + std::unique_ptr mFCLCollisionGeometryUserData; + + /// FCL collision object + std::unique_ptr mFCLCollisionObject; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTENGINEDATA_H_ diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.cpp b/dart/collision/fcl_mesh/FCLMeshEngine.cpp new file mode 100644 index 0000000000000..d492521565afe --- /dev/null +++ b/dart/collision/fcl_mesh/FCLMeshEngine.cpp @@ -0,0 +1,539 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/fcl_mesh/FCLMeshEngine.h" + +#include +#include +#include +#include +#include + +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionNode.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h" + +namespace dart { +namespace collision { + +namespace { + +// Collision data stores the collision request and the result given by +// collision algorithm. +struct CollisionData +{ + // Collision request + fcl::CollisionRequest request; + + // Collision result + fcl::CollisionResult result; + + // FCL collision detector +// FCLMeshEngine* collisionDetector; + + // Whether the collision iteration can stop + bool done; + + bool checkAllCollisions; + + CollisionData(); +}; + +bool collisionCallBack(fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata); + +void postProcess(const fcl::CollisionResult& fclResult, Result& result); + +int evalContactPosition( + const fcl::Contact& fclContact, + const fcl::BVHModel* mesh1, + const fcl::BVHModel* mesh2, + const fcl::Transform3f& transform1, + const fcl::Transform3f& transform2, + Eigen::Vector3d* contactPosition1, + Eigen::Vector3d* contactPosition2); + +int FFtest( + const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, + const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, + fcl::Vec3f* res1, fcl::Vec3f* res2); + +double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); + +void convert(const Option& option, fcl::CollisionRequest& fclRequest); + +void convert(const fcl::CollisionResult& fclResult, Result& result); + +} // anonymous namespace + + + +//============================================================================== +const std::string& FCLMeshEngine::getType() const +{ + return getTypeStatic(); +} + +//============================================================================== +FCLMeshEnginePtr FCLMeshEngine::create() +{ + return FCLMeshEnginePtr(new FCLMeshEngine()); +} + +//============================================================================== +const std::string& FCLMeshEngine::getTypeStatic() +{ + static const std::string& type("FCL"); + return type; +} + +//============================================================================== +CollisionObjectEngineDataPtr FCLMeshEngine::createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) +{ + return std::make_shared(parent, shape); +} + +//============================================================================== +CollisionGroupEngineDataPtr FCLMeshEngine::createCollisionGroupData( + const CollisionObjects& collObjects) +{ + return std::make_shared(collObjects); +} + +//============================================================================== +bool FCLMeshEngine::detect(CollisionObject* object1, + CollisionObject* object2, + const Option& option, + Result& result) +{ + result.contacts.clear(); + + assert(object1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(object2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + + object1->updateEngineData(); + object2->updateEngineData(); + + auto data1 = static_cast(object1->getEngineData()); + auto data2 = static_cast(object2->getEngineData()); + + auto fclCollObj1 = data1->getFCLCollisionObject(); + auto fclCollObj2 = data2->getFCLCollisionObject(); + + CollisionData collData; + convert(option, collData.request); + + collisionCallBack(fclCollObj1, fclCollObj2, &collData); + + convert(collData.result, result); + + return !result.contacts.empty(); +} + +//============================================================================== +bool FCLMeshEngine::detect(CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result) +{ + result.contacts.clear(); + + assert(object); + assert(group); + assert(object->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(group->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + + object->updateEngineData(); + group->updateEngineData(); + + auto objData = static_cast(object->getEngineData()); + auto groupData = static_cast(group->getEngineData()); + + auto fclObject = objData->getFCLCollisionObject(); + auto broadPhaseAlg = groupData->getFCLCollisionManager(); + + CollisionData collData; + convert(option, collData.request); + + broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); + + convert(collData.result, result); + + return !result.contacts.empty(); +} + +//============================================================================== +bool FCLMeshEngine::detect(CollisionGroup* group, + const Option& option, Result& result) +{ + result.contacts.clear(); + + assert(group); + assert(group->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + + group->updateEngineData(); + + auto data = static_cast(group->getEngineData()); + + auto broadPhaseAlg = data->getFCLCollisionManager(); + + CollisionData collData; + convert(option, collData.request); + + broadPhaseAlg->collide(&collData, collisionCallBack); + + convert(collData.result, result); + + return !result.contacts.empty(); +} + +//============================================================================== +bool FCLMeshEngine::detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) +{ + result.contacts.clear(); + + assert(group1); + assert(group2); + assert(group1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(group2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + + group1->updateEngineData(); + group2->updateEngineData(); + + auto data1 = static_cast(group1->getEngineData()); + auto data2 = static_cast(group2->getEngineData()); + + auto broadPhaseAlg1 = data1->getFCLCollisionManager(); + auto broadPhaseAlg2 = data2->getFCLCollisionManager(); + + CollisionData collData; + convert(option, collData.request); + + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); + + convert(collData.result, result); + + return !result.contacts.empty(); +} + + + +namespace { + +//============================================================================== +CollisionData::CollisionData() +{ + done = false; +} + +//============================================================================== +bool collisionCallBack(fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata) +{ + CollisionData* collData = static_cast(cdata); + + const fcl::CollisionRequest& request = collData->request; + fcl::CollisionResult& result = collData->result; +// FCLMeshEngine* cd = collData->collisionDetector; + // TODO(JS): take filter object instead of collision detector + + if (collData->done) + return true; + + // Filtering +// if (!cd->isCollidable(cd->findCollisionNode(o1), cd->findCollisionNode(o2))) +// return collData->done; + // TODO(JS): disabled until other functionalities are implemented + + // Perform narrow-phase detection + fcl::collide(o1, o2, request, result); + + if (!request.enable_cost + && (result.isCollision()) + && ((result.numContacts() >= request.num_max_contacts) + || !collData->checkAllCollisions)) + { + collData->done = true; + } + + return collData->done; +} + +//============================================================================== +void postProcess(const fcl::CollisionResult& fclResult, Result& result) +{ + result.contacts.clear(); + + int numCoplanarContacts = 0; + int numNoContacts = 0; + int numContacts = 0; + + std::vector unfiltered; + unfiltered.reserve(fclResult.numContacts()); + + for (auto k = 0u; k < fclResult.numContacts(); ++k) + { + // for each pair of intersecting triangles, we create two contact points + Contact pair1, pair2; + const fcl::Contact& c = fclResult.getContact(k); + + FCLCollisionGeometryUserData* userData1 + = static_cast(c.o1->getUserData()); + FCLCollisionGeometryUserData* userData2 + = static_cast(c.o2->getUserData()); + assert(userData1); + assert(userData2); + + auto collisionObject1 = userData1->mCollisionObject; + auto collisionObject2 = userData2->mCollisionObject; + + int contactResult = evalContactPosition( + c, + static_cast*>(c.o1), + static_cast*>(c.o2), + FCLTypes::convertTransform(collisionObject1->getTransform()), + FCLTypes::convertTransform(collisionObject2->getTransform()), + &pair1.point, &pair2.point); + + if (contactResult == COPLANAR_CONTACT) + { + numCoplanarContacts++; + + if (numContacts > 2) + continue; + } + else if (contactResult == NO_CONTACT) + { + numNoContacts++; + + continue; + } + else + { + numContacts++; + } + + pair1.normal = FCLTypes::convertVector3(-fclResult.getContact(k).normal); + pair2.normal = pair1.normal; + pair1.penetrationDepth = c.penetration_depth; + pair1.triID1 = c.b1; + pair1.triID2 = c.b2; + + pair1.shape1 = userData1->mShape; + pair1.shape2 = userData2->mShape; + pair1.collisionObject1 = collisionObject1; + pair1.collisionObject2 = collisionObject2; + + pair2 = pair1; + unfiltered.push_back(pair1); + unfiltered.push_back(pair2); + } + + const double ZERO = 0.000001; + const double ZERO2 = ZERO*ZERO; + + std::vector markForDeletion(unfiltered.size(), false); + + // mark all the repeated points + for (unsigned int k = 0; k < unfiltered.size(); k++) + { + for (unsigned int l = k + 1; l < unfiltered.size(); l++) + { + const Eigen::Vector3d diff = unfiltered[k].point - unfiltered[l].point; + + if (diff.dot(diff) < 3 * ZERO2) + { + markForDeletion[k] = true; + break; + } + } + } + + // remove all the co-linear contact points + for (auto k = 0u; k < unfiltered.size(); ++k) + { + if (markForDeletion[k]) + continue; + + for (auto l = 0u; l < unfiltered.size(); ++l) + { + if (l == k || markForDeletion[l]) + continue; + + if (markForDeletion[k]) + break; + + for (auto m = l + 1u; m < unfiltered.size(); ++m) + { + if (k == m) + continue; + + const Eigen::Vector3d va = unfiltered[k].point - unfiltered[l].point; + const Eigen::Vector3d vb = unfiltered[k].point - unfiltered[m].point; + const Eigen::Vector3d v = va.cross(vb); + + if (v.dot(v) < ZERO2 && va.dot(vb) < 0) + { + markForDeletion[k] = true; + break; + } + } + } + } + + for (size_t k = 0; k < unfiltered.size(); k++) + { + if (!markForDeletion[k]) + result.contacts.push_back(unfiltered[k]); + } +} + +//============================================================================== +int evalContactPosition( + const fcl::Contact& fclContact, + const fcl::BVHModel* mesh1, + const fcl::BVHModel* mesh2, + const fcl::Transform3f& transform1, + const fcl::Transform3f& transform2, + Eigen::Vector3d* contactPosition1, + Eigen::Vector3d* contactPosition2) +{ + int id1 = fclContact.b1; + int id2 = fclContact.b2; + fcl::Triangle tri1 = mesh1->tri_indices[id1]; + fcl::Triangle tri2 = mesh2->tri_indices[id2]; + + fcl::Vec3f v1, v2, v3, p1, p2, p3; + v1 = mesh1->vertices[tri1[0]]; + v2 = mesh1->vertices[tri1[1]]; + v3 = mesh1->vertices[tri1[2]]; + + p1 = mesh2->vertices[tri2[0]]; + p2 = mesh2->vertices[tri2[1]]; + p3 = mesh2->vertices[tri2[2]]; + + fcl::Vec3f contact1, contact2; + v1 = transform1.transform(v1); + v2 = transform1.transform(v2); + v3 = transform1.transform(v3); + p1 = transform2.transform(p1); + p2 = transform2.transform(p2); + p3 = transform2.transform(p3); + int testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); + + if (testRes == COPLANAR_CONTACT) + { + double area1 = triArea(v1, v2, v3); + double area2 = triArea(p1, p2, p3); + + if (area1 < area2) + contact1 = v1 + v2 + v3; + else + contact1 = p1 + p2 + p3; + + contact1[0] /= 3.0; + contact1[1] /= 3.0; + contact1[2] /= 3.0; + contact2 = contact1; + } + + *contactPosition1 = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); + *contactPosition2 = Eigen::Vector3d(contact2[0], contact2[1], contact2[2]); + + return testRes; +} + +//============================================================================== +int FFtest( + const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, + const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, + fcl::Vec3f* res1, fcl::Vec3f* res2) +{ + float U0[3], U1[3], U2[3], V0[3], V1[3], V2[3], RES1[3], RES2[3]; + SET(U0, r1); + SET(U1, r2); + SET(U2, r3); + SET(V0, R1); + SET(V1, R2); + SET(V2, R3); + + int contactResult = tri_tri_intersect(V0, V1, V2, U0, U1, U2, RES1, RES2); + + SET((*res1), RES1); + SET((*res2), RES2); + + return contactResult; +} + +//============================================================================== +double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) +{ + fcl::Vec3f a = p2 - p1; + fcl::Vec3f b = p3 - p1; + double aMag = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]; + double bMag = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]; + double dp = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + double area = 0.5 * sqrt(aMag * bMag - dp * dp); + + return area; +} + +//============================================================================== +void convert(const Option& fclResult, fcl::CollisionRequest& fclRequest) +{ + fclRequest.num_max_contacts = fclResult.maxNumContacts; + fclRequest.enable_contact = fclResult.enableContact; +#if FCL_VERSION_AT_LEAST(0,3,0) + fclRequest.gjk_solver_type = fcl::GST_LIBCCD; +#endif +} + +//============================================================================== +void convert(const fcl::CollisionResult& fclResult, Result& result) +{ + postProcess(fclResult, result); +} + +} // anonymous namespace + +} // namespace collision +} // namespace dart diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.h b/dart/collision/fcl_mesh/FCLMeshEngine.h new file mode 100644 index 0000000000000..20601e0baf020 --- /dev/null +++ b/dart/collision/fcl_mesh/FCLMeshEngine.h @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_FCLMESHENGINE_H_ +#define DART_COLLISION_FCL_FCLMESHENGINE_H_ + +#include + +#include "dart/collision/Engine.h" + +namespace dart { +namespace collision { + +class FCLCollisionGroup; + +/// FCL Collision detection engine +class FCLMeshEngine : public Engine +{ +public: + + static FCLMeshEnginePtr create(); + + /// Return engine type "FCL" + static const std::string& getTypeStatic(); + + // Documentation inherit + const std::string& getType() const override; + + // Documentation inherit + CollisionObjectEngineDataPtr createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) override; + + // Documentation inherit + CollisionGroupEngineDataPtr createCollisionGroupData( + const CollisionObjects& collObjects) override; + + // Documentation inherit + bool detect(CollisionObject* object1, CollisionObject* object2, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroup* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; + +protected: + + FCLMeshEngine() = default; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_FCLMESHENGINE_H_ diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 8c940f7be5b9c..b33353c1b1052 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -41,6 +41,8 @@ #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" +#include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION @@ -63,6 +65,8 @@ using namespace dynamics; //============================================================================== ConstraintSolver::ConstraintSolver(double _timeStep) : mCollisionDetector(new collision::FCLMeshCollisionDetector()), + mCollisionEngine(collision::FCLEngine::create()), + mCollisionGroup(new collision::CollisionGroup(mCollisionEngine)), mTimeStep(_timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { @@ -86,6 +90,7 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& _skeleton) { mSkeletons.push_back(_skeleton); mCollisionDetector->addSkeleton(_skeleton); + //mCollisionGroup-> mConstrainedGroups.reserve(mSkeletons.size()); } else diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index 830d46a7fccfd..88e4f07eb4a66 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -44,6 +44,7 @@ #include "dart/constraint/SmartPointer.h" #include "dart/constraint/ConstraintBase.h" #include "dart/collision/CollisionDetector.h" +#include "dart/collision/Engine.h" namespace dart { @@ -138,6 +139,12 @@ class ConstraintSolver /// Collision detector collision::CollisionDetector* mCollisionDetector; + /// Collision detection engine + std::shared_ptr mCollisionEngine; + + /// Collision group + std::unique_ptr mCollisionGroup; + /// Time step double mTimeStep; diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 209d7bbc11547..0db5461a89f96 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -46,6 +46,7 @@ #include "dart/math/math.h" #include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/fcl_mesh/FCLMeshEngine.h" #include "dart/dynamics/dynamics.h" #include "dart/simulation/simulation.h" #include "dart/utils/utils.h" @@ -568,6 +569,7 @@ void testFreeCollisionObjects() TEST_F(COLLISION, FreeCollisionObjects) { testFreeCollisionObjects(); + testFreeCollisionObjects(); } //============================================================================== @@ -603,6 +605,7 @@ void testBodyNodes() TEST_F(COLLISION, BodyNodeNodes) { testBodyNodes(); + testBodyNodes(); } //============================================================================== @@ -631,6 +634,7 @@ void testSkeletons() TEST_F(COLLISION, Skeletons) { testSkeletons(); + testSkeletons(); } //============================================================================== From 88d56abe9cf27becbd40a879b5d83ff4b68e1895 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 16 Feb 2016 16:46:35 -0500 Subject: [PATCH 06/67] Improve memory management for objects of new collision detection API --- dart/collision/CollisionGroup.cpp | 69 +++++-- dart/collision/CollisionGroup.h | 29 ++- dart/collision/CollisionGroupEngineData.h | 22 ++- dart/collision/CollisionObject.cpp | 5 +- dart/collision/CollisionObject.h | 5 +- dart/collision/Engine.h | 40 +++- dart/collision/fcl/FCLCollisionDetector.cpp | 10 +- .../fcl/FCLCollisionGroupEngineData.cpp | 34 +++- .../fcl/FCLCollisionGroupEngineData.h | 19 +- dart/collision/fcl/FCLCollisionNode.cpp | 12 +- dart/collision/fcl/FCLCollisionNode.h | 6 +- .../fcl/FCLCollisionObjectEngineData.cpp | 20 +- .../fcl/FCLCollisionObjectEngineData.h | 7 +- dart/collision/fcl/FCLEngine.cpp | 166 +++++++++-------- dart/collision/fcl/FCLEngine.h | 6 +- .../FCLMeshCollisionGroupEngineData.cpp | 35 +++- .../FCLMeshCollisionGroupEngineData.h | 19 +- .../FCLMeshCollisionObjectEngineData.cpp | 20 +- .../FCLMeshCollisionObjectEngineData.h | 6 +- dart/collision/fcl_mesh/FCLMeshEngine.cpp | 174 ++++++++++-------- dart/collision/fcl_mesh/FCLMeshEngine.h | 8 +- dart/dynamics/CollisionDetector.cpp | 26 ++- dart/dynamics/CollisionDetector.h | 43 +---- dart/dynamics/CollisionGroup.cpp | 59 ++++++ dart/dynamics/CollisionGroup.h | 60 ++++++ 25 files changed, 594 insertions(+), 306 deletions(-) create mode 100644 dart/dynamics/CollisionGroup.cpp create mode 100644 dart/dynamics/CollisionGroup.h diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 4123d6e8f7c52..3a0d5ca4e016b 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -46,29 +46,43 @@ namespace collision { //============================================================================== CollisionGroup::CollisionGroup( - const EnginePtr engine, - const CollisionGroup::CollisionObjects& collObjects) + const EnginePtr& engine, + const CollisionGroup::CollisionObjectPtrs& collObjects) : mEngine(engine), mCollisionObjects(collObjects), - mEngineData(mEngine->createCollisionGroupData(mCollisionObjects)) + mEngineData(mEngine->createCollisionGroupData(mCollisionObjects).release()) { assert(mEngine); } //============================================================================== -CollisionGroup::CollisionGroup( - const EnginePtr& engine, - const CollisionObjectPtr& collObject) - : mEngine(engine), - mCollisionObjects{collObject} +CollisionGroup::CollisionGroup(const CollisionGroup& other) + : mEngine(other.mEngine), + mCollisionObjects(other.mCollisionObjects), + mEngineData(other.mEngineData->clone(mCollisionObjects)) { - assert(mEngine); - assert(collObject); + // Do nothing +} + +//============================================================================== +CollisionGroup& CollisionGroup::operator=(const CollisionGroup& other) +{ + copy(other); + return *this; } //============================================================================== CollisionGroup::~CollisionGroup() { + // Do nothing +} + +//============================================================================== +void CollisionGroup::copy(const CollisionGroup& other) +{ + mEngine = other.mEngine; + mCollisionObjects = other.mCollisionObjects; + mEngineData.reset(other.mEngineData->clone(mCollisionObjects).release()); } //============================================================================== @@ -78,20 +92,45 @@ Engine* CollisionGroup::getEngine() const } //============================================================================== -void CollisionGroup::addCollisionObject(const CollisionObjectPtr& object) +bool CollisionGroup::hasCollisionObject( + const CollisionGroup::CollisionObjectPtr& object) const { - auto found = std::find(mCollisionObjects.begin(), - mCollisionObjects.end(), object) + return std::find(mCollisionObjects.begin(), + mCollisionObjects.end(), object) != mCollisionObjects.end(); +} - if (found) +//============================================================================== +void CollisionGroup::addCollisionObject(const CollisionObjectPtr& object) +{ + if (hasCollisionObject(object)) { // TODO(JS): print warning message return; } mCollisionObjects.push_back(object); - mEngineData->notifyCollisionObjectAdded(object.get()); + mEngineData->addCollisionObject(object); +} + +//============================================================================== +void CollisionGroup::addCollisionObjects( + const CollisionGroup::CollisionObjectPtrs& objects) +{ + bool added = false; + + for (auto object : objects) + { + if (!hasCollisionObject(object)) + { + added = true; + mCollisionObjects.push_back(object); + mEngineData->addCollisionObject(object, false); + } + } + + if (added) + mEngineData->init(); } //============================================================================== diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 1f344b3449515..2e641b88486ee 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -40,38 +40,49 @@ #include #include "dart/collision/Engine.h" +#include "dart/collision/CollisionGroupEngineData.h" namespace dart { namespace collision { class Engine; class CollisionNode; -class CollisionGroupEngineData; class CollisionGroup { public: using CollisionObjectPtr = std::shared_ptr; - using CollisionObjects = std::vector; - - /// Constructor - CollisionGroup(const EnginePtr engine, - const CollisionObjects& collObjects = CollisionObjects()); + using CollisionObjectPtrs = std::vector; /// Constructor CollisionGroup(const EnginePtr& engine, - const CollisionObjectPtr& collObject); + const CollisionObjectPtrs& collObjects = CollisionObjectPtrs()); + + /// Copy constructor + CollisionGroup(const CollisionGroup& other); /// Destructor virtual ~CollisionGroup(); + /// Assignment operator + CollisionGroup& operator=(const CollisionGroup& other); + + /// Copy another CollisionGroup into this CollisionGroup + void copy(const CollisionGroup& other); + /// Return collision detection engine associated with this CollisionGroup Engine* getEngine() const; + /// Return true if this CollisionGroup contains given object + bool hasCollisionObject(const CollisionObjectPtr& object) const; + /// Add collision object to this CollisionGroup void addCollisionObject(const CollisionObjectPtr& object); + /// Add collision objects to this CollisionGroup + void addCollisionObjects(const CollisionObjectPtrs& objects); + /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); @@ -102,10 +113,10 @@ class CollisionGroup // TODO(JS): Engine* ? /// Collision objects - CollisionObjects mCollisionObjects; + CollisionObjectPtrs mCollisionObjects; /// Engine specific data - std::shared_ptr mEngineData; + std::unique_ptr mEngineData; }; // TODO(JS): make this class iterable for collision objects diff --git a/dart/collision/CollisionGroupEngineData.h b/dart/collision/CollisionGroupEngineData.h index c8c5eebb74b4a..57593a01c0d68 100644 --- a/dart/collision/CollisionGroupEngineData.h +++ b/dart/collision/CollisionGroupEngineData.h @@ -37,6 +37,9 @@ #ifndef DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ #define DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ +#include +#include "dart/collision/SmartPointer.h" + namespace dart { namespace collision { @@ -45,13 +48,26 @@ class CollisionObject; class CollisionGroupEngineData { public: - virtual void update() = 0; - virtual void notifyCollisionObjectAdded(CollisionObject* object) = 0; + using CollisionObjectPtrs = std::vector; + + virtual void init() = 0; + + virtual void addCollisionObject(const CollisionObjectPtr& object, + bool init = true) = 0; +// virtual void addCollisionObjects(const CollisionObjectPtr& object) = 0; // TODO(JS): notifyCollisionObjectUpdated()? - virtual void notifyCollisionObjectRemoved(CollisionObject* object) = 0; + virtual void removeCollisionObject(const CollisionObjectPtr& object, + bool init = true) = 0; + + /// Update engine data. This function will be called ahead of every collision + /// checking + virtual void update() = 0; + + virtual std::unique_ptr clone( + const CollisionObjectPtrs& collObjects) const = 0; }; diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 3f0e5686984e5..5212532b36fad 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -84,12 +84,11 @@ void CollisionObject::updateEngineData() CollisionObject::CollisionObject(const EnginePtr& engine, const dynamics::ShapePtr& shape) : mEngine(engine), - mShape(shape) + mShape(shape), + mEngineData(mEngine->createCollisionObjectData(this, mShape).release()) { assert(mEngine); assert(mShape); - - mEngineData = mEngine->createCollisionObjectData(this, mShape); } //============================================================================== diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 30a2a1ddad328..b45679ae56b1f 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -41,12 +41,11 @@ #include "dart/collision/Engine.h" #include "dart/collision/SmartPointer.h" +#include "dart/collision/CollisionObjectEngineData.h" namespace dart { namespace collision { -class CollisionObjectEngineData; - class CollisionObject { public: @@ -95,7 +94,7 @@ class CollisionObject dynamics::ShapePtr mShape; /// Collision detection engine specific data - std::shared_ptr mEngineData; + std::unique_ptr mEngineData; }; diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h index 988dcffe4ad40..437da8f8b20fb 100644 --- a/dart/collision/Engine.h +++ b/dart/collision/Engine.h @@ -71,21 +71,31 @@ class Engine { public: - using CollisionObjects = std::vector; + using CollisionObjectPtrs = std::vector; + + /// Create a collision object + template + std::unique_ptr createCollisionObject( + const dynamics::ShapePtr& shape, + const Args&... args); + + /// Create a collision group + template + std::unique_ptr createCollisionGroup( + const CollisionObjectPtrs& collObjects, + const Args&... args); /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; /// Create collision detection engine specific data for CollisionObject - virtual CollisionObjectEngineDataPtr createCollisionObjectData( + virtual std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) = 0; - // TODO(JS): shared_ptr /// Create collision detection engine specific data for CollisionGroup - virtual CollisionGroupEngineDataPtr createCollisionGroupData( - const CollisionObjects& collObjects) = 0; - // TODO(JS): shared_ptr + virtual std::unique_ptr createCollisionGroupData( + const CollisionObjectPtrs& collObjects) = 0; /// Perform collision detection for object1-object2. virtual bool detect(CollisionObject* object1, CollisionObject* object2, @@ -109,6 +119,24 @@ class Engine }; +//============================================================================== +template +std::unique_ptr Engine::createCollisionObject( + const dynamics::ShapePtr& shape, const Args&... args) +{ + return std::unique_ptr( + new CollisionObjectType(this, shape, args...)); +} + +//============================================================================== +template +std::unique_ptr Engine::createCollisionGroup( + const Engine::CollisionObjectPtrs& collObjects, const Args&... args) +{ + return std::unique_ptr( + new CollisionGroupType(this, collObjects, args...)); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index b709d69ec8188..e7cd79110a36a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -56,9 +56,9 @@ namespace collision { //============================================================================== // Collision data stores the collision request and the result given by // collision algorithm. -struct CollisionData +struct FCLCollisionData { - CollisionData() { done = false; } + FCLCollisionData() { done = false; } // Collision request fcl::CollisionRequest request; @@ -78,7 +78,7 @@ bool collisionCallBack(fcl::CollisionObject* _o1, fcl::CollisionObject* _o2, void* _cdata) { - CollisionData* cdata = static_cast(_cdata); + FCLCollisionData* cdata = static_cast(_cdata); const fcl::CollisionRequest& request = cdata->request; fcl::CollisionResult& result = cdata->result; FCLCollisionDetector* cd = cdata->collisionDetector; @@ -174,7 +174,7 @@ bool FCLCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, static_cast(collNode)->updateFCLCollisionObjects(); mBroadPhaseAlg->update(); - CollisionData collData; + FCLCollisionData collData; collData.request.enable_contact = _calculateContactPoints; // TODO: Uncomment below once we strict to use fcl 0.3.0 or greater // collData.request.gjk_solver_type = fcl::GST_LIBCCD; @@ -257,7 +257,7 @@ CollisionNode* FCLCollisionDetector::findCollisionNode( FCLCollisionNode* FCLCollisionDetector::findCollisionNode( const fcl::CollisionObject* _fclCollObj) const { - FCLCollisionGeometryUserData* userData = static_cast(_fclCollObj->getUserData()); + FCLCollisionObjectUserData* userData = static_cast(_fclCollObj->getUserData()); return userData->mFclCollNode; } diff --git a/dart/collision/fcl/FCLCollisionGroupEngineData.cpp b/dart/collision/fcl/FCLCollisionGroupEngineData.cpp index 355e15be75b08..2232ace7e2fce 100644 --- a/dart/collision/fcl/FCLCollisionGroupEngineData.cpp +++ b/dart/collision/fcl/FCLCollisionGroupEngineData.cpp @@ -57,27 +57,45 @@ FCLCollisionGroupEngineData::FCLCollisionGroupEngineData( } //============================================================================== -void FCLCollisionGroupEngineData::update() +std::unique_ptr +FCLCollisionGroupEngineData::clone(const CollisionObjectPtrs& collObjects) const { - mBroadPhaseAlg->update(); + return std::unique_ptr( + new FCLCollisionGroupEngineData(collObjects)); } //============================================================================== -void FCLCollisionGroupEngineData::notifyCollisionObjectAdded( - CollisionObject* object) +void FCLCollisionGroupEngineData::init() +{ + mBroadPhaseAlg->setup(); +} + +//============================================================================== +void FCLCollisionGroupEngineData::addCollisionObject( + const CollisionObjectPtr& object, const bool init) { auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); - mBroadPhaseAlg->setup(); + + if (init) + this->init(); } //============================================================================== -void FCLCollisionGroupEngineData::notifyCollisionObjectRemoved( - CollisionObject* object) +void FCLCollisionGroupEngineData::removeCollisionObject( + const CollisionObjectPtr& object, const bool init) { auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); - mBroadPhaseAlg->setup(); + + if (init) + this->init(); +} + +//============================================================================== +void FCLCollisionGroupEngineData::update() +{ + mBroadPhaseAlg->update(); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionGroupEngineData.h b/dart/collision/fcl/FCLCollisionGroupEngineData.h index fdcc3f8621e5e..a80acf3ba6dc0 100644 --- a/dart/collision/fcl/FCLCollisionGroupEngineData.h +++ b/dart/collision/fcl/FCLCollisionGroupEngineData.h @@ -46,26 +46,35 @@ namespace dart { namespace collision { class CollisionObject; -class FCLCollisionGeometryUserData; +class FCLCollisionObjectUserData; class FCLCollisionGroupEngineData : public CollisionGroupEngineData { public: using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; - using CollisionObjects = CollisionGroup::CollisionObjects; + using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor FCLCollisionGroupEngineData(const CollisionObjects& collObjects); // Documentation inherited - void update() override; + std::unique_ptr clone( + const CollisionObjectPtrs& collObjects) const override; + + // Documentation inherited + void init() override; // Documentation inherited - void notifyCollisionObjectAdded(CollisionObject* object) override; + void addCollisionObject(const CollisionObjectPtr& object, + const bool init) override; // Documentation inherited - void notifyCollisionObjectRemoved(CollisionObject* object) override; + void removeCollisionObject(const CollisionObjectPtr& object, + const bool init) override; + + // Documentation inherited + void update() override; /// Return FCL collision manager that is also a broad-phase algorithm FCLCollisionManager* getFCLCollisionManager() const; diff --git a/dart/collision/fcl/FCLCollisionNode.cpp b/dart/collision/fcl/FCLCollisionNode.cpp index 4e6587fcb370a..3fce0e9454d33 100644 --- a/dart/collision/fcl/FCLCollisionNode.cpp +++ b/dart/collision/fcl/FCLCollisionNode.cpp @@ -468,7 +468,7 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) } //============================================================================== -FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( +FCLCollisionObjectUserData::FCLCollisionObjectUserData( FCLCollisionNode* fclCollNode, dynamics::BodyNode* bodyNode, const dynamics::ShapePtr& shape) @@ -480,7 +480,7 @@ FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( } //============================================================================== -FCLCollisionGeometryUserData::FCLCollisionGeometryUserData( +FCLCollisionObjectUserData::FCLCollisionObjectUserData( CollisionObject* collisionObject, const dynamics::ShapePtr& shape) : mCollisionObject(collisionObject), @@ -611,7 +611,7 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) assert(nullptr != fclCollGeom); fcl::CollisionObject* fclCollObj = new fcl::CollisionObject(fclCollGeom, getFCLTransform(i)); - fclCollGeom->setUserData(new FCLCollisionGeometryUserData(this, _bodyNode, shape)); + fclCollGeom->setUserData(new FCLCollisionObjectUserData(this, _bodyNode, shape)); mCollisionObjects.push_back(fclCollObj); } } @@ -623,7 +623,7 @@ FCLCollisionNode::~FCLCollisionNode() { assert(collObj); - delete static_cast(collObj->getUserData()); + delete static_cast(collObj->getUserData()); delete collObj; } } @@ -671,8 +671,8 @@ void FCLCollisionNode::updateFCLCollisionObjects() for (auto& fclCollObj : mCollisionObjects) { - FCLCollisionGeometryUserData* userData - = static_cast(fclCollObj->collisionGeometry()->getUserData()); + FCLCollisionObjectUserData* userData + = static_cast(fclCollObj->collisionGeometry()->getUserData()); BodyNode* bodyNode = userData->mBodyNode; Shape* shape = userData->mShape.get(); diff --git a/dart/collision/fcl/FCLCollisionNode.h b/dart/collision/fcl/FCLCollisionNode.h index 65247afe43c84..733e0e9113621 100644 --- a/dart/collision/fcl/FCLCollisionNode.h +++ b/dart/collision/fcl/FCLCollisionNode.h @@ -61,18 +61,18 @@ namespace collision { class FCLCollisionNode; class CollisionObject; -struct FCLCollisionGeometryUserData +struct FCLCollisionObjectUserData { FCLCollisionNode* mFclCollNode; CollisionObject* mCollisionObject; dynamics::BodyNode* mBodyNode; dynamics::ShapePtr mShape; - FCLCollisionGeometryUserData(FCLCollisionNode* mFclCollNode, + FCLCollisionObjectUserData(FCLCollisionNode* mFclCollNode, dynamics::BodyNode* mBodyNode, const dynamics::ShapePtr& mShape); - FCLCollisionGeometryUserData(CollisionObject* mFclCollNode, + FCLCollisionObjectUserData(CollisionObject* mFclCollNode, const dynamics::ShapePtr& mShape); }; diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.cpp b/dart/collision/fcl/FCLCollisionObjectEngineData.cpp index 2e79332ff427f..183367ba5b703 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.cpp @@ -478,8 +478,7 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) //============================================================================== boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ShapePtr& shape, - FCLCollisionGeometryUserData* userData) + const dynamics::ShapePtr& shape) { using dynamics::Shape; using dynamics::BoxShape; @@ -592,8 +591,6 @@ boost::shared_ptr createFCLCollisionGeometry( } } - fclCollGeom->setUserData(userData); - return fclCollGeom; } @@ -604,13 +601,11 @@ FCLCollisionObjectEngineData::FCLCollisionObjectEngineData( CollisionObject* parent, const dynamics::ShapePtr& shape) : CollisionObjectEngineData(), - mFCLCollisionGeometryUserData( - new FCLCollisionGeometryUserData(parent, shape)), - mFCLCollisionObject( - new fcl::CollisionObject( - createFCLCollisionGeometry(shape, mFCLCollisionGeometryUserData.get()))) + mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), + mFCLCollisionObject(new fcl::CollisionObject( + createFCLCollisionGeometry(shape))) { - // Do nothing + mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); } //============================================================================== @@ -623,8 +618,7 @@ void FCLCollisionObjectEngineData::updateTransform(const Eigen::Isometry3d& tf) //============================================================================== void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) { - auto fclCollGeom = createFCLCollisionGeometry( - shape, mFCLCollisionGeometryUserData.get()); + auto fclCollGeom = createFCLCollisionGeometry(shape); mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); } @@ -636,7 +630,7 @@ void FCLCollisionObjectEngineData::update() using dart::dynamics::Shape; using dart::dynamics::SoftMeshShape; - auto collisionObject = mFCLCollisionGeometryUserData->mCollisionObject; + auto collisionObject = mFCLCollisionObjectUserData->mCollisionObject; auto shape = collisionObject->getShape().get(); if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.h b/dart/collision/fcl/FCLCollisionObjectEngineData.h index 1d6a273a25241..9a7727d19132e 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.h +++ b/dart/collision/fcl/FCLCollisionObjectEngineData.h @@ -48,7 +48,7 @@ namespace dart { namespace collision { class CollisionObject; -class FCLCollisionGeometryUserData; +class FCLCollisionObjectUserData; class FCLCollisionObjectEngineData : public CollisionObjectEngineData { @@ -73,10 +73,13 @@ class FCLCollisionObjectEngineData : public CollisionObjectEngineData protected: /// FCL collision geometry user data - std::unique_ptr mFCLCollisionGeometryUserData; + std::unique_ptr mFCLCollisionObjectUserData; /// FCL collision object std::unique_ptr mFCLCollisionObject; + // Note: We can consider sharing fcl::CollisionGeometry with other + // CollisionObjects that are associated with the same Shape of DART to avoid + // unnecessary copy of fcl::CollisionGeometry. }; diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index 23bc9aa94359e..0dfc6a390917b 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -52,36 +52,52 @@ namespace collision { namespace { -// Collision data stores the collision request and the result given by -// collision algorithm. -struct CollisionData -{ - // Collision request - fcl::CollisionRequest request; +bool collisionCallBack(fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata); - // Collision result - fcl::CollisionResult result; +void postProcess(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result); - // FCL collision detector -// FCLEngine* collisionDetector; +void convertOption(const Option& fclOption, fcl::CollisionRequest& request); - // Whether the collision iteration can stop - bool done; +Contact convertContact(const fcl::Contact& fclContact, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2); - bool checkAllCollisions; +/// Collision data stores the collision request and the result given by +/// collision algorithm. +struct FCLCollisionData +{ + /// FCL collision request + fcl::CollisionRequest mFclRequest; - CollisionData(); -}; + /// FCL collision result + fcl::CollisionResult mFclResult; -bool collisionCallBack(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata); + /// Collision option of DART + const Option* mOption; -void convert(const Option& fclResult, fcl::CollisionRequest& request); + /// Collision result of DART + Result* mResult; -void convert(const fcl::CollisionResult& fclResult, Result& result); + /// Whether the collision iteration can stop + bool done; -void convert(const fcl::Contact& fclContact, Contact& contact); + /// Constructor + FCLCollisionData( + const Option* option = nullptr, + Result* result = nullptr) + : mOption(option), + mResult(result), + done(false) + { + if (mOption) + convertOption(*mOption, mFclRequest); + } +}; } // anonymous namespace @@ -107,18 +123,20 @@ const std::string& FCLEngine::getTypeStatic() } //============================================================================== -CollisionObjectEngineDataPtr FCLEngine::createCollisionObjectData( +std::unique_ptr FCLEngine::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { - return std::make_shared(parent, shape); + return std::unique_ptr( + new FCLCollisionObjectEngineData(parent, shape)); } //============================================================================== -CollisionGroupEngineDataPtr FCLEngine::createCollisionGroupData( - const CollisionObjects& collObjects) +std::unique_ptr FCLEngine::createCollisionGroupData( + const CollisionObjectPtrs& collObjects) { - return std::make_shared(collObjects); + return std::unique_ptr( + new FCLCollisionGroupEngineData(collObjects)); } //============================================================================== @@ -141,13 +159,9 @@ bool FCLEngine::detect(CollisionObject* object1, auto fclCollObj1 = data1->getFCLCollisionObject(); auto fclCollObj2 = data2->getFCLCollisionObject(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); collisionCallBack(fclCollObj1, fclCollObj2, &collData); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -171,13 +185,9 @@ bool FCLEngine::detect(CollisionObject* object, CollisionGroup* group, auto fclObject = objData->getFCLCollisionObject(); auto broadPhaseAlg = groupData->getFCLCollisionManager(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -196,13 +206,9 @@ bool FCLEngine::detect(CollisionGroup* group, auto broadPhaseAlg = data->getFCLCollisionManager(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(&collData, collisionCallBack); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -226,13 +232,9 @@ bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, auto broadPhaseAlg1 = data1->getFCLCollisionManager(); auto broadPhaseAlg2 = data2->getFCLCollisionManager(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -240,21 +242,16 @@ bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, namespace { -//============================================================================== -CollisionData::CollisionData() -{ - done = false; -} - //============================================================================== bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { - CollisionData* collData = static_cast(cdata); + FCLCollisionData* collData = static_cast(cdata); - const fcl::CollisionRequest& request = collData->request; - fcl::CollisionResult& result = collData->result; + const fcl::CollisionRequest& fclRequest = collData->mFclRequest; + fcl::CollisionResult& fclResult = collData->mFclResult; + Result& result = *(collData->mResult); // FCLEngine* cd = collData->collisionDetector; // TODO(JS): take filter object instead of collision detector @@ -267,44 +264,53 @@ bool collisionCallBack(fcl::CollisionObject* o1, // TODO(JS): disabled until other functionalities are implemented // Perform narrow-phase detection - fcl::collide(o1, o2, request, result); + fcl::collide(o1, o2, fclRequest, fclResult); - if (!request.enable_cost - && (result.isCollision()) - && ((result.numContacts() >= request.num_max_contacts) - || !collData->checkAllCollisions)) + if (!fclRequest.enable_cost + && (fclResult.isCollision()) + && ((fclResult.numContacts() >= fclRequest.num_max_contacts))) + //|| !collData->checkAllCollisions)) + // TODO(JS): checkAllCollisions should be in FCLCollisionData { collData->done = true; } + postProcess(fclResult, o1, o2, result); + return collData->done; } //============================================================================== -void convert(const Option& fclResult, fcl::CollisionRequest& request) +void postProcess(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result) { - request.num_max_contacts = fclResult.maxNumContacts; - request.enable_contact = fclResult.enableContact; -#if FCL_VERSION_AT_LEAST(0,3,0) - request.gjk_solver_type = fcl::GST_LIBCCD; -#endif + auto numContacts = fclResult.numContacts(); + for (auto i = 0u; i < numContacts; ++i) + { + const auto fclContact = fclResult.getContact(i); + result.contacts.push_back(convertContact(fclContact, o1, o2)); + } } //============================================================================== -void convert(const fcl::CollisionResult& fclResult, Result& result) +void convertOption(const Option& fclOption, fcl::CollisionRequest& request) { - result.contacts.resize(fclResult.numContacts()); - - // TODO(JS): Check if there is contact point sufficiently close to the new - // contact point - - for (auto i = 0u; i < result.contacts.size(); ++i) - convert(fclResult.getContact(i), result.contacts[i]); + request.num_max_contacts = fclOption.maxNumContacts; + request.enable_contact = fclOption.enableContact; +#if FCL_VERSION_AT_LEAST(0,3,0) + request.gjk_solver_type = fcl::GST_LIBCCD; +#endif } //============================================================================== -void convert(const fcl::Contact& fclContact, Contact& contact) +Contact convertContact(const fcl::Contact& fclContact, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2) { + Contact contact; + Eigen::Vector3d point = FCLTypes::convertVector3(fclContact.pos); contact.point = point; @@ -313,16 +319,18 @@ void convert(const fcl::Contact& fclContact, Contact& contact) contact.triID1 = fclContact.b1; contact.triID2 = fclContact.b2; - FCLCollisionGeometryUserData* userData1 - = static_cast(fclContact.o1->getUserData()); - FCLCollisionGeometryUserData* userData2 - = static_cast(fclContact.o2->getUserData()); + FCLCollisionObjectUserData* userData1 + = static_cast(o1->getUserData()); + FCLCollisionObjectUserData* userData2 + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); contact.shape1 = userData1->mShape; contact.shape2 = userData2->mShape; contact.collisionObject1 = userData1->mCollisionObject; contact.collisionObject2 = userData2->mCollisionObject; + + return contact; } } // anonymous namespace diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index df92e1df69abb..4ff01437ba815 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -60,13 +60,13 @@ class FCLEngine : public Engine const std::string& getType() const override; // Documentation inherit - CollisionObjectEngineDataPtr createCollisionObjectData( + std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; // Documentation inherit - CollisionGroupEngineDataPtr createCollisionGroupData( - const CollisionObjects& collObjects) override; + std::unique_ptr createCollisionGroupData( + const CollisionObjectPtrs& collObjects) override; // Documentation inherit bool detect(CollisionObject* object1, CollisionObject* object2, diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp index c01693ecc3669..c6e1e3d6d4514 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp @@ -57,27 +57,46 @@ FCLMeshCollisionGroupEngineData::FCLMeshCollisionGroupEngineData( } //============================================================================== -void FCLMeshCollisionGroupEngineData::update() +std::unique_ptr +FCLMeshCollisionGroupEngineData::clone( + const CollisionGroupEngineData::CollisionObjectPtrs& collObjects) const { - mBroadPhaseAlg->update(); + return std::unique_ptr( + new FCLMeshCollisionGroupEngineData(collObjects)); } //============================================================================== -void FCLMeshCollisionGroupEngineData::notifyCollisionObjectAdded( - CollisionObject* object) +void FCLMeshCollisionGroupEngineData::init() +{ + mBroadPhaseAlg->setup(); +} + +//============================================================================== +void FCLMeshCollisionGroupEngineData::addCollisionObject( + const CollisionObjectPtr& object, const bool init) { auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); - mBroadPhaseAlg->setup(); + + if (init) + this->init(); } //============================================================================== -void FCLMeshCollisionGroupEngineData::notifyCollisionObjectRemoved( - CollisionObject* object) +void FCLMeshCollisionGroupEngineData::removeCollisionObject( + const CollisionObjectPtr& object, const bool init) { auto data = static_cast(object->getEngineData()); mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); - mBroadPhaseAlg->setup(); + + if (init) + this->init(); +} + +//============================================================================== +void FCLMeshCollisionGroupEngineData::update() +{ + mBroadPhaseAlg->update(); } //============================================================================== diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h index 81131a704d201..a8adcd78de0c0 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h @@ -46,26 +46,35 @@ namespace dart { namespace collision { class CollisionObject; -class FCLCollisionGeometryUserData; +class FCLCollisionObjectUserData; class FCLMeshCollisionGroupEngineData : public CollisionGroupEngineData { public: using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; - using CollisionObjects = CollisionGroup::CollisionObjects; + using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor FCLMeshCollisionGroupEngineData(const CollisionObjects& collObjects); // Documentation inherited - void update() override; + std::unique_ptr clone( + const CollisionObjectPtrs& collObjects) const override; + + // Documentation inherited + void init() override; // Documentation inherited - void notifyCollisionObjectAdded(CollisionObject* object) override; + void addCollisionObject(const CollisionObjectPtr& object, + const bool init) override; // Documentation inherited - void notifyCollisionObjectRemoved(CollisionObject* object) override; + void removeCollisionObject(const CollisionObjectPtr& object, + const bool init) override; + + // Documentation inherited + void update() override; /// Return FCL collision manager that is also a broad-phase algorithm FCLCollisionManager* getFCLCollisionManager() const; diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp index e8939163606c4..66cc2fbdcad33 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.cpp @@ -470,8 +470,7 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) //============================================================================== boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ShapePtr& shape, - FCLCollisionGeometryUserData* userData) + const dynamics::ShapePtr& shape) { using ::dart::dynamics::Shape; using ::dart::dynamics::BoxShape; @@ -551,8 +550,6 @@ boost::shared_ptr createFCLCollisionGeometry( } } - fclCollGeom->setUserData(userData); - return fclCollGeom; } @@ -563,13 +560,11 @@ FCLMeshCollisionObjectEngineData::FCLMeshCollisionObjectEngineData( CollisionObject* parent, const dynamics::ShapePtr& shape) : CollisionObjectEngineData(), - mFCLCollisionGeometryUserData( - new FCLCollisionGeometryUserData(parent, shape)), - mFCLCollisionObject( - new fcl::CollisionObject( - createFCLCollisionGeometry(shape, mFCLCollisionGeometryUserData.get()))) + mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), + mFCLCollisionObject(new fcl::CollisionObject( + createFCLCollisionGeometry(shape))) { - // Do nothing + mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); } //============================================================================== @@ -584,8 +579,7 @@ void FCLMeshCollisionObjectEngineData::updateTransform( void FCLMeshCollisionObjectEngineData::updateShape( const dynamics::ShapePtr& shape) { - auto fclCollGeom = createFCLCollisionGeometry( - shape, mFCLCollisionGeometryUserData.get()); + auto fclCollGeom = createFCLCollisionGeometry(shape); mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); @@ -598,7 +592,7 @@ void FCLMeshCollisionObjectEngineData::update() using dart::dynamics::Shape; using dart::dynamics::SoftMeshShape; - auto collisionObject = mFCLCollisionGeometryUserData->mCollisionObject; + auto collisionObject = mFCLCollisionObjectUserData->mCollisionObject; auto shape = collisionObject->getShape().get(); if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h index 841c451f785d0..7f96d9e178133 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h @@ -48,7 +48,7 @@ namespace dart { namespace collision { class CollisionObject; -class FCLCollisionGeometryUserData; +class FCLCollisionObjectUserData; class FCLMeshCollisionObjectEngineData : public CollisionObjectEngineData { @@ -56,7 +56,7 @@ class FCLMeshCollisionObjectEngineData : public CollisionObjectEngineData /// Constructor FCLMeshCollisionObjectEngineData(CollisionObject* parent, - const dynamics::ShapePtr& shape); + const dynamics::ShapePtr& shape); // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; @@ -73,7 +73,7 @@ class FCLMeshCollisionObjectEngineData : public CollisionObjectEngineData protected: /// FCL collision geometry user data - std::unique_ptr mFCLCollisionGeometryUserData; + std::unique_ptr mFCLCollisionObjectUserData; /// FCL collision object std::unique_ptr mFCLCollisionObject; diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.cpp b/dart/collision/fcl_mesh/FCLMeshEngine.cpp index d492521565afe..f1e4e3e0884e3 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.cpp +++ b/dart/collision/fcl_mesh/FCLMeshEngine.cpp @@ -54,32 +54,48 @@ namespace collision { namespace { -// Collision data stores the collision request and the result given by -// collision algorithm. -struct CollisionData -{ - // Collision request - fcl::CollisionRequest request; +bool collisionCallBack(fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + void* cdata); - // Collision result - fcl::CollisionResult result; +void postProcess(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result); - // FCL collision detector -// FCLMeshEngine* collisionDetector; +void convertOption(const Option& option, fcl::CollisionRequest& fclRequest); - // Whether the collision iteration can stop - bool done; +/// Collision data stores the collision request and the result given by +/// collision algorithm. +struct FCLCollisionData +{ + /// FCL collision request + fcl::CollisionRequest mFclRequest; - bool checkAllCollisions; + /// FCL collision result + fcl::CollisionResult mFclResult; - CollisionData(); -}; + /// Collision option of DART + const Option* mOption; -bool collisionCallBack(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata); + /// Collision result of DART + Result* mResult; + + /// Whether the collision iteration can stop + bool done; -void postProcess(const fcl::CollisionResult& fclResult, Result& result); + /// Constructor + FCLCollisionData( + const Option* option = nullptr, + Result* result = nullptr) + : mOption(option), + mResult(result), + done(false) + { + if (mOption) + convertOption(*mOption, mFclRequest); + } +}; int evalContactPosition( const fcl::Contact& fclContact, @@ -97,10 +113,6 @@ int FFtest( double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); -void convert(const Option& option, fcl::CollisionRequest& fclRequest); - -void convert(const fcl::CollisionResult& fclResult, Result& result); - } // anonymous namespace @@ -120,23 +132,27 @@ FCLMeshEnginePtr FCLMeshEngine::create() //============================================================================== const std::string& FCLMeshEngine::getTypeStatic() { - static const std::string& type("FCL"); + static const std::string& type("FCLMesh"); return type; } //============================================================================== -CollisionObjectEngineDataPtr FCLMeshEngine::createCollisionObjectData( +std::unique_ptr +FCLMeshEngine::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { - return std::make_shared(parent, shape); + return std::unique_ptr( + new FCLMeshCollisionObjectEngineData(parent, shape)); } //============================================================================== -CollisionGroupEngineDataPtr FCLMeshEngine::createCollisionGroupData( - const CollisionObjects& collObjects) +std::unique_ptr +FCLMeshEngine::createCollisionGroupData( + const CollisionObjectPtrs& collObjects) { - return std::make_shared(collObjects); + return std::unique_ptr( + new FCLMeshCollisionGroupEngineData(collObjects)); } //============================================================================== @@ -159,13 +175,9 @@ bool FCLMeshEngine::detect(CollisionObject* object1, auto fclCollObj1 = data1->getFCLCollisionObject(); auto fclCollObj2 = data2->getFCLCollisionObject(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); collisionCallBack(fclCollObj1, fclCollObj2, &collData); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -189,13 +201,9 @@ bool FCLMeshEngine::detect(CollisionObject* object, CollisionGroup* group, auto fclObject = objData->getFCLCollisionObject(); auto broadPhaseAlg = groupData->getFCLCollisionManager(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -214,13 +222,9 @@ bool FCLMeshEngine::detect(CollisionGroup* group, auto broadPhaseAlg = data->getFCLCollisionManager(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(&collData, collisionCallBack); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -244,13 +248,9 @@ bool FCLMeshEngine::detect(CollisionGroup* group1, CollisionGroup* group2, auto broadPhaseAlg1 = data1->getFCLCollisionManager(); auto broadPhaseAlg2 = data2->getFCLCollisionManager(); - CollisionData collData; - convert(option, collData.request); - + FCLCollisionData collData(&option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); - convert(collData.result, result); - return !result.contacts.empty(); } @@ -258,22 +258,17 @@ bool FCLMeshEngine::detect(CollisionGroup* group1, CollisionGroup* group2, namespace { -//============================================================================== -CollisionData::CollisionData() -{ - done = false; -} - //============================================================================== bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { - CollisionData* collData = static_cast(cdata); + FCLCollisionData* collData = static_cast(cdata); - const fcl::CollisionRequest& request = collData->request; - fcl::CollisionResult& result = collData->result; -// FCLMeshEngine* cd = collData->collisionDetector; + const fcl::CollisionRequest& fclRequest = collData->mFclRequest; + fcl::CollisionResult& fclResult = collData->mFclResult; + Result& result = *(collData->mResult); +// FCLEngine* cd = collData->collisionDetector; // TODO(JS): take filter object instead of collision detector if (collData->done) @@ -285,21 +280,27 @@ bool collisionCallBack(fcl::CollisionObject* o1, // TODO(JS): disabled until other functionalities are implemented // Perform narrow-phase detection - fcl::collide(o1, o2, request, result); + fcl::collide(o1, o2, fclRequest, fclResult); - if (!request.enable_cost - && (result.isCollision()) - && ((result.numContacts() >= request.num_max_contacts) - || !collData->checkAllCollisions)) + if (!fclRequest.enable_cost + && (fclResult.isCollision()) + && ((fclResult.numContacts() >= fclRequest.num_max_contacts))) + //|| !collData->checkAllCollisions)) + // TODO(JS): checkAllCollisions should be in FCLCollisionData { collData->done = true; } + postProcess(fclResult, o1, o2, result); + return collData->done; } //============================================================================== -void postProcess(const fcl::CollisionResult& fclResult, Result& result) +void postProcess(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result) { result.contacts.clear(); @@ -316,10 +317,10 @@ void postProcess(const fcl::CollisionResult& fclResult, Result& result) Contact pair1, pair2; const fcl::Contact& c = fclResult.getContact(k); - FCLCollisionGeometryUserData* userData1 - = static_cast(c.o1->getUserData()); - FCLCollisionGeometryUserData* userData2 - = static_cast(c.o2->getUserData()); + FCLCollisionObjectUserData* userData1 + = static_cast(o1->getUserData()); + FCLCollisionObjectUserData* userData2 + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); @@ -518,19 +519,42 @@ double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) } //============================================================================== -void convert(const Option& fclResult, fcl::CollisionRequest& fclRequest) +void convertOption(const Option& fclOption, fcl::CollisionRequest& request) { - fclRequest.num_max_contacts = fclResult.maxNumContacts; - fclRequest.enable_contact = fclResult.enableContact; + request.num_max_contacts = fclOption.maxNumContacts; + request.enable_contact = fclOption.enableContact; #if FCL_VERSION_AT_LEAST(0,3,0) - fclRequest.gjk_solver_type = fcl::GST_LIBCCD; + request.gjk_solver_type = fcl::GST_LIBCCD; #endif } //============================================================================== -void convert(const fcl::CollisionResult& fclResult, Result& result) +Contact convertContact(const fcl::Contact& fclContact, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2) { - postProcess(fclResult, result); + Contact contact; + + Eigen::Vector3d point = FCLTypes::convertVector3(fclContact.pos); + + contact.point = point; + contact.normal = -FCLTypes::convertVector3(fclContact.normal); + contact.penetrationDepth = fclContact.penetration_depth; + contact.triID1 = fclContact.b1; + contact.triID2 = fclContact.b2; + + FCLCollisionObjectUserData* userData1 + = static_cast(o1->getUserData()); + FCLCollisionObjectUserData* userData2 + = static_cast(o2->getUserData()); + assert(userData1); + assert(userData2); + contact.shape1 = userData1->mShape; + contact.shape2 = userData2->mShape; + contact.collisionObject1 = userData1->mCollisionObject; + contact.collisionObject2 = userData2->mCollisionObject; + + return contact; } } // anonymous namespace diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.h b/dart/collision/fcl_mesh/FCLMeshEngine.h index 20601e0baf020..dd7c141162d8f 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.h +++ b/dart/collision/fcl_mesh/FCLMeshEngine.h @@ -53,20 +53,20 @@ class FCLMeshEngine : public Engine static FCLMeshEnginePtr create(); - /// Return engine type "FCL" + /// Return engine type "FCLMesh" static const std::string& getTypeStatic(); // Documentation inherit const std::string& getType() const override; // Documentation inherit - CollisionObjectEngineDataPtr createCollisionObjectData( + std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; // Documentation inherit - CollisionGroupEngineDataPtr createCollisionGroupData( - const CollisionObjects& collObjects) override; + std::unique_ptr createCollisionGroupData( + const CollisionObjectPtrs& collObjects) override; // Documentation inherit bool detect(CollisionObject* object1, CollisionObject* object2, diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index 9d9862f4055cf..bfafad8be6ab1 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -82,7 +82,7 @@ BodyNodePtr ShapeNodeCollisionObject::getBodyNode() const //============================================================================== std::shared_ptr -CollisionDetector::createCollisionNode( +CollisionDetector::createCollisionObject( const collision::EnginePtr engine, const ShapePtr& shape, const BodyNodePtr& bodyNode) @@ -90,5 +90,29 @@ CollisionDetector::createCollisionNode( return std::make_shared(engine, shape, bodyNode); } +//============================================================================== +std::vector +CollisionDetector::createCollisionObjects( + const collision::EnginePtr& engine, + const SkeletonPtr& skel) +{ + std::vector objects; + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + { + auto bodyNode = skel->getBodyNode(i); + auto numColShapes = bodyNode->getNumCollisionShapes(); + + for (auto j = 0u; j < numColShapes; ++j) + { + auto shape = bodyNode->getCollisionShape(j); + objects.push_back(createCollisionObject(engine, shape, bodyNode)); + } + } + + return objects; +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index d5649d2802e6d..22b2ba383117a 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -68,6 +68,7 @@ class ShapeNodeCollisionObject : public collision::CollisionObject protected: dynamics::BodyNodePtr mBodyNode; + // TODO(JS): this should be changed to ShapeNode }; @@ -75,11 +76,16 @@ class CollisionDetector { public: - static std::shared_ptr createCollisionNode( + static std::shared_ptr createCollisionObject( const collision::EnginePtr engine, const dynamics::ShapePtr& shape, const BodyNodePtr& bodyNode); + static std::vector + createCollisionObjects( + const collision::EnginePtr& engine, + const dynamics::SkeletonPtr& skel); + template static bool detect(const dynamics::ShapePtr& shape1, const BodyNodePtr& body1, @@ -94,11 +100,6 @@ class CollisionDetector const collision::Option& option, collision::Result& result); - static std::vector - createShapeNodeCollisionObjects( - const collision::EnginePtr& engine, - const dynamics::SkeletonPtr& skel); - }; //============================================================================== @@ -125,10 +126,8 @@ bool CollisionDetector::detect( { auto engine = ColDecEngine::create(); - std::vector objects1 - = createShapeNodeCollisionObjects(engine, skel1); - std::vector objects2 - = createShapeNodeCollisionObjects(engine, skel2); + auto objects1 = createCollisionObjects(engine, skel1); + auto objects2 = createCollisionObjects(engine, skel2); auto group1 = collision::CollisionGroup(engine, objects1); auto group2 = collision::CollisionGroup(engine, objects2); @@ -136,30 +135,6 @@ bool CollisionDetector::detect( return group1.detect(&group2, option, result); } -//============================================================================== -std::vector -CollisionDetector::createShapeNodeCollisionObjects( - const collision::EnginePtr& engine, - const dynamics::SkeletonPtr& skel) -{ - std::vector objects; - - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - { - auto bodyNode = skel->getBodyNode(i); - auto numColShapes = bodyNode->getNumCollisionShapes(); - - for (auto j = 0u; j < numColShapes; ++j) - { - auto shape = bodyNode->getCollisionShape(j); - objects.push_back(createCollisionNode(engine, shape, bodyNode)); - } - } - - return objects; -} - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/CollisionGroup.cpp b/dart/dynamics/CollisionGroup.cpp new file mode 100644 index 0000000000000..470b232d18994 --- /dev/null +++ b/dart/dynamics/CollisionGroup.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/CollisionGroup.h" +#include "dart/dynamics/CollisionDetector.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +CollisionGroup::CollisionGroup(const collision::EnginePtr& engine) + : collision::CollisionGroup(engine) +{ + +} + +//============================================================================== +void CollisionGroup::addCollisionObjects(const SkeletonPtr& skeleton) +{ + auto objects = CollisionDetector::createCollisionObjects(mEngine, skeleton); + + collision::CollisionGroup::addCollisionObjects(objects); +} + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/CollisionGroup.h b/dart/dynamics/CollisionGroup.h new file mode 100644 index 0000000000000..8646e1042d93e --- /dev/null +++ b/dart/dynamics/CollisionGroup.h @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_COLLISIONGROUP_H_ +#define DART_DYNAMICS_COLLISIONGROUP_H_ + +#include + +#include "dart/collision/CollisionGroup.h" + +namespace dart { +namespace dynamics { + +class CollisionGroup : public collision::CollisionGroup +{ +public: + + CollisionGroup(const collision::EnginePtr& engine); + + void addCollisionObjects(const SkeletonPtr& skeleton); + +}; + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_COLLISIONDETECTOR_H_ From 7adf6b8b14ee3b2f16715d6ca200e326626eb429 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 18 Feb 2016 09:38:10 -0500 Subject: [PATCH 07/67] Suppress unused variable warnings in bullet collision detector --- dart/collision/bullet/BulletCollisionDetector.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index b70933f69d53e..5860b84f1a44e 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -151,8 +151,8 @@ CollisionNode* BulletCollisionDetector::createCollisionNode( } //============================================================================== -bool BulletCollisionDetector::detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints) +bool BulletCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, + bool /*_calculateContactPoints*/) { // Update all the transformations of the collision nodes for (size_t i = 0; i < mCollisionNodes.size(); ++i) @@ -217,9 +217,9 @@ bool BulletCollisionDetector::detectCollision(bool _checkAllCollisions, } //============================================================================== -bool BulletCollisionDetector::detectCollision(CollisionNode* _node1, - CollisionNode* _node2, - bool _calculateContactPoints) +bool BulletCollisionDetector::detectCollision(CollisionNode* /*_node1*/, + CollisionNode* /*_node2*/, + bool /*_calculateContactPoints*/) { assert(false); return false; From 10397b0df5b8df73614853db7c8eacab6a00af5f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 25 Feb 2016 22:18:54 -0500 Subject: [PATCH 08/67] More implementation of new collision checking API and applying to the existing code base --- dart/collision/CMakeLists.txt | 10 +- dart/collision/CollisionDetector.cpp | 316 ++------ dart/collision/CollisionDetector.h | 201 ++--- dart/collision/CollisionGroup.cpp | 115 ++- dart/collision/CollisionGroup.h | 47 +- .../CollisionGroupData.cpp} | 23 +- ...GroupEngineData.h => CollisionGroupData.h} | 20 +- dart/collision/CollisionNode.h | 81 -- dart/collision/CollisionObject.cpp | 42 +- dart/collision/CollisionObject.h | 37 +- .../CollisionObjectData.cpp} | 30 +- ...jectEngineData.h => CollisionObjectData.h} | 18 +- dart/collision/Contact.cpp | 43 ++ dart/collision/Contact.h | 90 +++ dart/collision/Engine.cpp | 11 +- dart/collision/Engine.h | 71 +- dart/collision/Option.cpp | 51 ++ dart/collision/Option.h | 66 ++ dart/collision/Result.cpp | 43 ++ .../collision/{CollisionNode.cpp => Result.h} | 36 +- dart/collision/SmartPointer.h | 7 +- dart/collision/detail/Engine.h | 48 ++ dart/collision/fcl/FCLCollisionDetector.cpp | 265 ------- dart/collision/fcl/FCLCollisionDetector.h | 90 --- .../FCLCollisionGroupData.cpp} | 50 +- .../FCLCollisionGroupData.h} | 17 +- dart/collision/fcl/FCLCollisionNode.cpp | 725 ------------------ dart/collision/fcl/FCLCollisionNode.h | 133 ---- ...ineData.cpp => FCLCollisionObjectData.cpp} | 29 +- ...tEngineData.h => FCLCollisionObjectData.h} | 27 +- dart/collision/fcl/FCLEngine.cpp | 209 +++-- dart/collision/fcl/FCLEngine.h | 22 +- .../fcl_mesh/FCLMeshCollisionDetector.cpp | 154 ---- .../fcl_mesh/FCLMeshCollisionDetector.h | 81 -- .../FCLMeshCollisionGroupData.cpp} | 51 +- .../FCLMeshCollisionGroupData.h} | 22 +- .../fcl_mesh/FCLMeshCollisionNode.cpp | 482 ------------ .../collision/fcl_mesh/FCLMeshCollisionNode.h | 244 ------ ...ata.cpp => FCLMeshCollisionObjectData.cpp} | 25 +- ...ineData.h => FCLMeshCollisionObjectData.h} | 15 +- dart/collision/fcl_mesh/FCLMeshEngine.cpp | 253 +++--- dart/collision/fcl_mesh/FCLMeshEngine.h | 22 +- dart/constraint/ConstraintSolver.cpp | 173 ++--- dart/constraint/ConstraintSolver.h | 37 +- dart/constraint/ContactConstraint.cpp | 9 +- dart/constraint/SoftContactConstraint.cpp | 14 +- dart/dynamics/CollisionDetector.cpp | 75 +- dart/dynamics/CollisionDetector.h | 118 ++- dart/dynamics/CollisionGroupGenerator.h | 72 -- .../CollisionDetector.h} | 16 +- dart/utils/SkelParser.cpp | 43 +- dart/utils/sdf/SoftSdfParser.cpp | 3 - tutorials/tutorialBiped-Finished.cpp | 5 +- tutorials/tutorialBiped.cpp | 5 +- tutorials/tutorialCollisions-Finished.cpp | 33 +- tutorials/tutorialDominoes-Finished.cpp | 37 +- unittests/testCollision.cpp | 50 +- 57 files changed, 1535 insertions(+), 3477 deletions(-) rename dart/{dynamics/CollisionGroup.cpp => collision/CollisionGroupData.cpp} (80%) rename dart/collision/{CollisionGroupEngineData.h => CollisionGroupData.h} (86%) delete mode 100644 dart/collision/CollisionNode.h rename dart/{dynamics/CollisionGroupGenerator.cpp => collision/CollisionObjectData.cpp} (77%) rename dart/collision/{CollisionObjectEngineData.h => CollisionObjectData.h} (85%) create mode 100644 dart/collision/Contact.cpp create mode 100644 dart/collision/Contact.h create mode 100644 dart/collision/Option.cpp create mode 100644 dart/collision/Option.h create mode 100644 dart/collision/Result.cpp rename dart/collision/{CollisionNode.cpp => Result.h} (76%) create mode 100644 dart/collision/detail/Engine.h delete mode 100644 dart/collision/fcl/FCLCollisionDetector.cpp delete mode 100644 dart/collision/fcl/FCLCollisionDetector.h rename dart/collision/{fcl_mesh/FCLMeshCollisionGroupEngineData.cpp => fcl/FCLCollisionGroupData.cpp} (70%) rename dart/collision/{fcl_mesh/FCLMeshCollisionGroupEngineData.h => fcl/FCLCollisionGroupData.h} (85%) delete mode 100644 dart/collision/fcl/FCLCollisionNode.cpp delete mode 100644 dart/collision/fcl/FCLCollisionNode.h rename dart/collision/fcl/{FCLCollisionObjectEngineData.cpp => FCLCollisionObjectData.cpp} (96%) rename dart/collision/fcl/{FCLCollisionObjectEngineData.h => FCLCollisionObjectData.h} (79%) delete mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp delete mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionDetector.h rename dart/collision/{fcl/FCLCollisionGroupEngineData.cpp => fcl_mesh/FCLMeshCollisionGroupData.cpp} (68%) rename dart/collision/{fcl/FCLCollisionGroupEngineData.h => fcl_mesh/FCLMeshCollisionGroupData.h} (80%) delete mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp delete mode 100644 dart/collision/fcl_mesh/FCLMeshCollisionNode.h rename dart/collision/fcl_mesh/{FCLMeshCollisionObjectEngineData.cpp => FCLMeshCollisionObjectData.cpp} (96%) rename dart/collision/fcl_mesh/{FCLMeshCollisionObjectEngineData.h => FCLMeshCollisionObjectData.h} (84%) delete mode 100644 dart/dynamics/CollisionGroupGenerator.h rename dart/dynamics/{CollisionGroup.h => detail/CollisionDetector.h} (87%) diff --git a/dart/collision/CMakeLists.txt b/dart/collision/CMakeLists.txt index 2e89eb2fd9a72..5763542bffa1c 100644 --- a/dart/collision/CMakeLists.txt +++ b/dart/collision/CMakeLists.txt @@ -1,17 +1,19 @@ # Search all header and source files file(GLOB srcs "*.cpp") file(GLOB hdrs "*.h") +file(GLOB detail_srcs "detail/*.cpp") +file(GLOB detail_hdrs "detail/*.h") # Add subdirectories -add_subdirectory(dart) +#add_subdirectory(dart) add_subdirectory(fcl) add_subdirectory(fcl_mesh) if(HAVE_BULLET_COLLISION) - add_subdirectory(bullet) +# add_subdirectory(bullet) endif() -set(dart_collision_hdrs ${hdrs} ${dart_collision_hdrs} PARENT_SCOPE) -set(dart_collision_srcs ${srcs} ${dart_collision_srcs} PARENT_SCOPE) +set(dart_collision_hdrs ${hdrs} "${dart_collision_hdrs};${detail_hdrs}" PARENT_SCOPE) +set(dart_collision_srcs ${srcs} "${dart_collision_srcs};${detail_srcs}" PARENT_SCOPE) # Library #dart_add_library(dart_collision ${srcs} ${hdrs}) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 73f99dd903701..4e3dd3c3ea59d 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -42,310 +42,110 @@ #include #include "dart/common/Console.h" +#include "dart/collision/Engine.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionGroup.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" -#include "dart/collision/CollisionNode.h" namespace dart { namespace collision { -CollisionDetector::CollisionDetector() - : mNumMaxContacts(100) { -} - -CollisionDetector::~CollisionDetector() { - for (size_t i = 0; i < mCollisionNodes.size(); i++) - delete mCollisionNodes[i]; -} - //============================================================================== -void CollisionDetector::addSkeleton(const dynamics::SkeletonPtr& _skeleton) +CollisionDetector::~CollisionDetector() { - assert(_skeleton != nullptr - && "Null pointer skeleton is not allowed to add to CollisionDetector."); - - if (containSkeleton(_skeleton) == false) - { - mSkeletons.push_back(_skeleton); - for (size_t i = 0; i < _skeleton->getNumBodyNodes(); ++i) - addCollisionSkeletonNode(_skeleton->getBodyNode(i)); - } - else - { - dtwarn << "Skeleton [" << _skeleton->getName() - << "] is already in CollisionDetector." << std::endl; - } + // Do nothing } //============================================================================== -void CollisionDetector::removeSkeleton(const dynamics::SkeletonPtr& _skeleton) +Engine* CollisionDetector::getEngine() { - assert(_skeleton != nullptr - && "Null pointer skeleton is not allowed to add to CollisionDetector."); - - if (containSkeleton(_skeleton)) - { - mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), _skeleton), - mSkeletons.end()); - for (size_t i = 0; i < _skeleton->getNumBodyNodes(); ++i) - removeCollisionSkeletonNode(_skeleton->getBodyNode(i)); - } - else - { - dtwarn << "Skeleton [" << _skeleton->getName() - << "] is not in CollisionDetector." << std::endl; - } + return mEngine.get(); } //============================================================================== -void CollisionDetector::removeAllSkeletons() +const Engine* CollisionDetector::getEngine() const { - for (size_t i = 0; i < mSkeletons.size(); ++i) - removeSkeleton(mSkeletons[i]); - - mSkeletons.clear(); -} - -void CollisionDetector::addCollisionSkeletonNode(dynamics::BodyNode* _bodyNode, - bool _isRecursive) { - assert(_bodyNode != nullptr && "Invalid body node."); - - // If this collision detector already has collision node for _bodyNode, then - // we do nothing. - if (getCollisionNode(_bodyNode) != nullptr) { - std::cout << "The collision detector already has a collision node for " - << "body node [" << _bodyNode->getName() << "]." << std::endl; - return; - } - - // Create collision node and set index - CollisionNode* collNode = createCollisionNode(_bodyNode); - collNode->setIndex(mCollisionNodes.size()); - - // Add the collision node to collision node list - mCollisionNodes.push_back(collNode); - - // Add the collision node to map (BodyNode -> CollisionNode) - mBodyCollisionMap[_bodyNode] = collNode; - - // Add collidable pairs for the collision node - mCollidablePairs.push_back( - std::vector(mCollisionNodes.size() - 1, true)); - - if (_isRecursive) { - for (size_t i = 0; i < _bodyNode->getNumChildBodyNodes(); i++) - addCollisionSkeletonNode(_bodyNode->getChildBodyNode(i), true); - } + return mEngine.get(); } -void CollisionDetector::removeCollisionSkeletonNode( - dynamics::BodyNode* _bodyNode, bool _isRecursive) { - assert(_bodyNode != nullptr && "Invalid body node."); - - // If a collision node is already created for _bodyNode, then we just return - CollisionNode* collNode = getCollisionNode(_bodyNode); - if (collNode == nullptr) { - std::cout << "The collision detector does not have any collision node " - << "for body node [" << _bodyNode->getName() << "]." - << std::endl; - return; - } - - // Update index of collision nodes. - size_t iCollNode = collNode->getIndex(); - for (size_t i = iCollNode + 1; i < mCollisionNodes.size(); ++i) - mCollisionNodes[i]->setIndex(mCollisionNodes[i]->getIndex() - 1); - - // Remove collNode from mCollisionNodes - mCollisionNodes.erase(remove(mCollisionNodes.begin(), mCollisionNodes.end(), - collNode), - mCollisionNodes.end()); - - // Remove collNode-_bodyNode pair from mBodyCollisionMap - mBodyCollisionMap.erase(_bodyNode); - - // Delete collNode - delete collNode; - - // Update mCollidablePairs - for (size_t i = iCollNode + 1; i < mCollidablePairs.size(); ++i) { - for (size_t j = 0; j < iCollNode; ++j) - mCollidablePairs[i-1][j] = mCollidablePairs[i][j]; - for (size_t j = iCollNode + 1; j < mCollidablePairs[i].size(); ++j) - mCollidablePairs[i-1][j-1] = mCollidablePairs[i][j]; - } - mCollidablePairs.pop_back(); - - if (_isRecursive) { - for (size_t i = 0; i < _bodyNode->getNumChildBodyNodes(); i++) - removeCollisionSkeletonNode(_bodyNode->getChildBodyNode(i), true); - } -} - -bool CollisionDetector::detectCollision(dynamics::BodyNode* _node1, - dynamics::BodyNode* _node2, - bool _calculateContactPoints) { - return detectCollision(getCollisionNode(_node1), - getCollisionNode(_node2), - _calculateContactPoints); -} - -size_t CollisionDetector::getNumContacts() { - return mContacts.size(); -} - -Contact& CollisionDetector::getContact(int _idx) { - return mContacts[_idx]; -} - -void CollisionDetector::clearAllContacts() { - mContacts.clear(); -} - -int CollisionDetector::getNumMaxContacts() const { - return mNumMaxContacts; -} - -void CollisionDetector::setNumMaxContacs(int _num) { - mNumMaxContacts = _num; -} - -void CollisionDetector::enablePair(dynamics::BodyNode* _node1, - dynamics::BodyNode* _node2) { - CollisionNode* collisionNode1 = getCollisionNode(_node1); - CollisionNode* collisionNode2 = getCollisionNode(_node2); - if (collisionNode1 && collisionNode2) - setPairCollidable(collisionNode1, collisionNode2, true); +//============================================================================== +std::shared_ptr CollisionDetector::createCollisionGroup( + const std::vector& objects) +{ + return std::make_shared(shared_from_this(), objects); } -void CollisionDetector::disablePair(dynamics::BodyNode* _node1, - dynamics::BodyNode* _node2) { - CollisionNode* collisionNode1 = getCollisionNode(_node1); - CollisionNode* collisionNode2 = getCollisionNode(_node2); - if (collisionNode1 && collisionNode2) - setPairCollidable(collisionNode1, collisionNode2, false); +//============================================================================== +std::shared_ptr CollisionDetector::createCollisionGroup() +{ + return std::make_shared(shared_from_this()); } //============================================================================== -bool CollisionDetector::isCollidable(const CollisionNode* _node1, - const CollisionNode* _node2) +bool CollisionDetector::detect( + CollisionObject* object1, CollisionObject* object2, + const Option& option, Result& result) { - dynamics::BodyNode* bn1 = _node1->getBodyNode(); - dynamics::BodyNode* bn2 = _node2->getBodyNode(); + assert(object1->getCollisionDetector() == object2->getCollisionDetector()); - if (!getPairCollidable(_node1, _node2)) - return false; + object1->updateEngineData(); + object2->updateEngineData(); - if (!bn1->isCollidable() || !bn2->isCollidable()) - return false; - - if (bn1->getSkeleton() == bn2->getSkeleton()) - { - if (bn1->getSkeleton()->isEnabledSelfCollisionCheck()) - { - if (isAdjacentBodies(bn1, bn2)) - { - if (!bn1->getSkeleton()->isEnabledAdjacentBodyCheck()) - return false; - } - } - else - { - return false; - } - } - - return true; + return mEngine->detect(object1->getEngineData(), object2->getEngineData(), + option, result); } //============================================================================== -bool CollisionDetector::containSkeleton(const dynamics::SkeletonPtr& _skeleton) +bool CollisionDetector::detect( + CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result) { - for (std::vector::const_iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) - { - if ((*it) == _skeleton) - return true; - } + assert(object->getCollisionDetector() == group->getCollisionDetector()); + + object->updateEngineData(); + group->updateEngineData(); - return false; + return mEngine->detect(object->getEngineData(), group->getEngineData(), + option, result); } -bool isValidIndex(const std::vector>& _collidablePairs, - const std::size_t _index1, - const std::size_t _index2) +//============================================================================== +bool CollisionDetector::detect( + CollisionGroup* group, CollisionObject* object, + const Option& option, Result& result) { - assert(_index1 >= _index2); - - if (_collidablePairs.size() > _index1 - && _collidablePairs[_index1].size() > _index2) - return true; - - return false; + return detect(object, group, option, result); } -bool CollisionDetector::getPairCollidable(const CollisionNode* _node1, - const CollisionNode* _node2) +//============================================================================== +bool CollisionDetector::detect( + CollisionGroup* group, const Option& option, Result& result) { - assert(_node1 != _node2); - - size_t index1 = _node1->getIndex(); - size_t index2 = _node2->getIndex(); - - if (index1 < index2) - std::swap(index1, index2); - - // Index validity check. The indices are not valid if the body nodes are not - // completely added to the collision detector yet. - if (!isValidIndex(mCollidablePairs, index1, index2)) - return false; + group->updateEngineData(); - if (index1 == index2) - return false; - - return mCollidablePairs[index1][index2]; + return mEngine->detect(group->getEngineData(), option, result); } -void CollisionDetector::setPairCollidable(const CollisionNode* _node1, - const CollisionNode* _node2, - bool _val) +//============================================================================== +bool CollisionDetector::detect( + CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) { - assert(_node1 != _node2); - - size_t index1 = _node1->getIndex(); - size_t index2 = _node2->getIndex(); + assert(group1->getCollisionDetector() == group2->getCollisionDetector()); - if (index1 < index2) - std::swap(index1, index2); + group1->updateEngineData(); + group2->updateEngineData(); - // Index validity check. The indices are not valid if the body nodes are not - // completely added to the collision detector yet. - if (!isValidIndex(mCollidablePairs, index1, index2)) - return; - - mCollidablePairs[index1][index2] = _val; + return mEngine->detect(group1->getEngineData(), group2->getEngineData(), + option, result); } -bool CollisionDetector::isAdjacentBodies( - const dynamics::BodyNode* _bodyNode1, - const dynamics::BodyNode* _bodyNode2) const +//============================================================================== +CollisionDetector::CollisionDetector(std::unique_ptr&& engine) + : mEngine(std::move(engine)) { - if ((_bodyNode1->getParentBodyNode() == _bodyNode2) - || (_bodyNode2->getParentBodyNode() == _bodyNode1)) - { - assert(_bodyNode1->getSkeleton() == _bodyNode2->getSkeleton()); - return true; - } - - return false; -} - -CollisionNode* CollisionDetector::getCollisionNode( - const dynamics::BodyNode* _bodyNode) { - if (mBodyCollisionMap.find(_bodyNode) != mBodyCollisionMap.end()) - return mBodyCollisionMap[_bodyNode]; - else - return nullptr; + assert(mEngine); } } // namespace collision diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 017c5368ba04e..60e7d661d4f19 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -43,7 +43,10 @@ #include -#include "dart/collision/CollisionNode.h" +#include "dart/collision/Contact.h" +#include "dart/collision/Option.h" +#include "dart/collision/Result.h" +#include "dart/collision/SmartPointer.h" #include "dart/dynamics/SmartPointer.h" namespace dart { @@ -51,165 +54,87 @@ namespace collision { class CollisionObject; -/// Contact information -struct Contact { - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// Contact point w.r.t. the world frame - Eigen::Vector3d point; - - /// Contact normal vector from bodyNode2 to bodyNode1 w.r.t. the world frame - Eigen::Vector3d normal; - - /// Contact force acting on bodyNode1 w.r.t. the world frame - /// - /// The contact force acting on bodyNode2 is -force, which is the opposite - /// direction of the force. - Eigen::Vector3d force; - - /// First colliding body node - dynamics::WeakBodyNodePtr bodyNode1; - - /// Second colliding body node - dynamics::WeakBodyNodePtr bodyNode2; - - /// First colliding shape of the first body node - dynamics::ShapePtr shape1; - - /// Second colliding shape of the first body node - dynamics::ShapePtr shape2; - - CollisionObject* collisionObject1; - CollisionObject* collisionObject2; - - /// Penetration depth - double penetrationDepth; - - // TODO(JS): triID1 will be deprecated when we don't use fcl_mesh - /// \brief - int triID1; - - // TODO(JS): triID2 will be deprecated when we don't use fcl_mesh - /// \brief - int triID2; - - // TODO(JS): userData is an experimental variable. - /// \brief User data. - void* userData; -}; - -/// \brief class CollisionDetector -class CollisionDetector +class CollisionDetector : public std::enable_shared_from_this { public: - /// \brief Constructor - CollisionDetector(); - - /// \brief Destructor - virtual ~CollisionDetector(); - /// \brief Add skeleton - virtual void addSkeleton(const dynamics::SkeletonPtr& _skeleton); + template + static CollisionDetectorPtr create(const Args&... args); - /// \brief Remove skeleton - virtual void removeSkeleton(const dynamics::SkeletonPtr& _skeleton); - - /// \brief Remove all skeletons - virtual void removeAllSkeletons(); - - // TODO(JS): Change accessibility to private - /// \brief - virtual void addCollisionSkeletonNode(dynamics::BodyNode* _bodyNode, - bool _isRecursive = false); - - // TODO(JS): Change accessibility to private - /// \brief - virtual void removeCollisionSkeletonNode(dynamics::BodyNode* _bodyNode, - bool _isRecursive = false); - - /// \brief - virtual CollisionNode* createCollisionNode(dynamics::BodyNode* _bodyNode) = 0; + /// Destructor + virtual ~CollisionDetector(); - /// \brief - void enablePair(dynamics::BodyNode* _node1, dynamics::BodyNode* _node2); + /// Return collision detection engine + Engine* getEngine(); - /// \brief - void disablePair(dynamics::BodyNode* _node1, dynamics::BodyNode* _node2); + /// Return (const) collision detection engine + const Engine* getEngine() const; - /// Return true if there exists at least one contact - /// \param[in] _checkAllCollision True to detect every collisions - /// \param[in] _calculateContactPoints True to get contact points - virtual bool detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints) = 0; + /// Create a collision object + template + std::shared_ptr createCollisionObject( + const dynamics::ShapePtr& shape, + const Args&... args); - /// Return true if there exists contacts between two bodies - /// \param[in] _calculateContactPoints True to get contact points - bool detectCollision(dynamics::BodyNode* _node1, dynamics::BodyNode* _node2, - bool _calculateContactPoints); + /// Create a collision group + std::shared_ptr createCollisionGroup( + const std::vector& objects); - /// \brief - size_t getNumContacts(); + /// Create a collision group + std::shared_ptr createCollisionGroup(); - /// \brief - Contact& getContact(int _idx); + /// Perform collision detection for object1-object2. + bool detect(CollisionObject* object1, CollisionObject* object2, + const Option& option, Result& result); - /// \brief - void clearAllContacts(); + /// Perform collision detection for object-group. + bool detect(CollisionObject* object, CollisionGroup* group, + const Option& option, Result& result); - /// \brief - int getNumMaxContacts() const; + /// Identical with detect(object, group, option, result) + bool detect(CollisionGroup* group, CollisionObject* object, + const Option& option, Result& result); - /// \brief - void setNumMaxContacs(int _num); + /// Perform collision detection for group. + bool detect(CollisionGroup* group, + const Option& option, Result& result); - /// \brief - bool isCollidable(const CollisionNode* _node1, const CollisionNode* _node2); + /// Perform collision detection for group1-group2. + bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result); protected: - /// \brief - virtual bool detectCollision(CollisionNode* _node1, CollisionNode* _node2, - bool _calculateContactPoints) = 0; - - /// \brief - std::vector mContacts; - - /// \brief - std::vector mCollisionNodes; - - /// \brief - int mNumMaxContacts; - /// \brief Skeleton array - std::vector mSkeletons; + /// Constructor + CollisionDetector(std::unique_ptr&& engine); -private: - /// \brief Return true if _skeleton is contained - bool containSkeleton(const dynamics::SkeletonPtr& _skeleton); - - /// \brief - bool getPairCollidable(const CollisionNode* _node1, - const CollisionNode* _node2); - - /// \brief - void setPairCollidable(const CollisionNode* _node1, - const CollisionNode* _node2, - bool _val); - - /// \brief Return true if _bodyNode1 and _bodyNode2 are adjacent bodies - bool isAdjacentBodies(const dynamics::BodyNode* _bodyNode1, - const dynamics::BodyNode* _bodyNode2) const; - - /// \brief - CollisionNode* getCollisionNode(const dynamics::BodyNode* _bodyNode); +protected: - /// \brief - std::map mBodyCollisionMap; + std::unique_ptr mEngine; - /// \brief - std::vector > mCollidablePairs; }; +//============================================================================== +template +CollisionDetectorPtr CollisionDetector::create(const Args&... args) +{ + auto engine = new EngineType(args...); + + return CollisionDetectorPtr(new CollisionDetector( + std::move(std::unique_ptr(engine)))); +} + +//============================================================================== +template +std::shared_ptr +CollisionDetector::createCollisionObject( + const dynamics::ShapePtr& shape, + const Args&... args) +{ + return std::make_shared( + shared_from_this(), shape, args...); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 3a0d5ca4e016b..6416a18ac02f7 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -37,7 +37,8 @@ #include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionGroupEngineData.h" +#include "dart/collision/CollisionGroupData.h" +#include "dart/collision/CollisionDetector.h" #include @@ -46,18 +47,19 @@ namespace collision { //============================================================================== CollisionGroup::CollisionGroup( - const EnginePtr& engine, + const CollisionDetectorPtr& collisionDetector, const CollisionGroup::CollisionObjectPtrs& collObjects) - : mEngine(engine), + : mCollisionDetector(collisionDetector), mCollisionObjects(collObjects), - mEngineData(mEngine->createCollisionGroupData(mCollisionObjects).release()) + mEngineData(mCollisionDetector->getEngine()->createCollisionGroupData( + mCollisionObjects).release()) { - assert(mEngine); + assert(mCollisionDetector); } //============================================================================== CollisionGroup::CollisionGroup(const CollisionGroup& other) - : mEngine(other.mEngine), + : mCollisionDetector(other.mCollisionDetector), mCollisionObjects(other.mCollisionObjects), mEngineData(other.mEngineData->clone(mCollisionObjects)) { @@ -80,15 +82,27 @@ CollisionGroup::~CollisionGroup() //============================================================================== void CollisionGroup::copy(const CollisionGroup& other) { - mEngine = other.mEngine; + mCollisionDetector = other.mCollisionDetector; mCollisionObjects = other.mCollisionObjects; mEngineData.reset(other.mEngineData->clone(mCollisionObjects).release()); } //============================================================================== -Engine* CollisionGroup::getEngine() const +void CollisionGroup::changeDetector( + const CollisionDetectorPtr& collisionDetector) { - return mEngine.get(); + if (mCollisionDetector == collisionDetector) + return; + +// mEngineData.reset( +// engine->createCollisionGroupData(getCollisionObjects()).release()); + // TODO(JS): Implement this! +} + +//============================================================================== +CollisionDetector* CollisionGroup::getCollisionDetector() const +{ + return mCollisionDetector.get(); } //============================================================================== @@ -123,9 +137,9 @@ void CollisionGroup::addCollisionObjects( { if (!hasCollisionObject(object)) { - added = true; mCollisionObjects.push_back(object); mEngineData->addCollisionObject(object, false); + added = true; } } @@ -133,28 +147,101 @@ void CollisionGroup::addCollisionObjects( mEngineData->init(); } +//============================================================================== +void CollisionGroup::removeCollisionObject( + const CollisionGroup::CollisionObjectPtr& object) +{ + if (!object) + return; + + auto result + = std::find(mCollisionObjects.begin(), mCollisionObjects.end(), object); + + if (mCollisionObjects.end() != result) + { + mCollisionObjects.erase(result); + mEngineData->removeCollisionObject(*result); + } +} + +//============================================================================== +void CollisionGroup::removeCollisionObjects( + const CollisionGroup::CollisionObjectPtrs& objects) +{ + for (auto object : objects) + removeCollisionObject(object); + // TODO(JS): there is a room for improving the perfomance +} + +//============================================================================== +void CollisionGroup::removeAllCollisionObjects() +{ + mCollisionObjects.clear(); + mEngineData->removeAllCollisionObjects(); +} + +//============================================================================== +const CollisionGroup::CollisionObjectPtrs& +CollisionGroup::getCollisionObjects() +{ + return mCollisionObjects; +} + +//============================================================================== +const CollisionGroup::ConstCollisionObjectPtrs +CollisionGroup::getCollisionObjects() const +{ + ConstCollisionObjectPtrs vec(mCollisionObjects.size()); + + for (auto i = 0u; i < mCollisionObjects.size(); ++i) + { + vec[i] = std::const_pointer_cast( + mCollisionObjects[i]); + } + + return vec; +} + +//============================================================================== +void CollisionGroup::unionGroup(const CollisionGroupPtr& other) +{ + if (!other) + return; + + this->addCollisionObjects(other->getCollisionObjects()); +} + +//============================================================================== +void CollisionGroup::subtractGroup(const CollisionGroupPtr& other) +{ + if (!other) + return; + + this->removeCollisionObjects(other->getCollisionObjects()); +} + //============================================================================== bool CollisionGroup::detect(const Option& option, Result& result) { - return mEngine->detect(this, option, result); + return mCollisionDetector->detect(this, option, result); } //============================================================================== bool CollisionGroup::detect(CollisionObject* object, const Option& option, Result& result) { - return mEngine->detect(object, this, option, result); + return mCollisionDetector->detect(object, this, option, result); } //============================================================================== bool CollisionGroup::detect(CollisionGroup* otherGroup, const Option& option, Result& result) { - return mEngine->detect(this, otherGroup, option, result); + return mCollisionDetector->detect(this, otherGroup, option, result); } //============================================================================== -CollisionGroupEngineData* CollisionGroup::getEngineData() +CollisionGroupData* CollisionGroup::getEngineData() { return mEngineData.get(); } diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 2e641b88486ee..5a3b9558d9cc8 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -40,7 +40,7 @@ #include #include "dart/collision/Engine.h" -#include "dart/collision/CollisionGroupEngineData.h" +#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { @@ -48,16 +48,19 @@ namespace collision { class Engine; class CollisionNode; +/// Heterogeneous collision group class CollisionGroup { public: using CollisionObjectPtr = std::shared_ptr; using CollisionObjectPtrs = std::vector; + using ConstCollisionObjectPtrs = std::vector; /// Constructor - CollisionGroup(const EnginePtr& engine, - const CollisionObjectPtrs& collObjects = CollisionObjectPtrs()); + CollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const CollisionObjectPtrs& collObjects = CollisionObjectPtrs()); /// Copy constructor CollisionGroup(const CollisionGroup& other); @@ -71,8 +74,11 @@ class CollisionGroup /// Copy another CollisionGroup into this CollisionGroup void copy(const CollisionGroup& other); + /// Change engine + void changeDetector(const CollisionDetectorPtr& collisionDetector); + /// Return collision detection engine associated with this CollisionGroup - Engine* getEngine() const; + CollisionDetector* getCollisionDetector() const; /// Return true if this CollisionGroup contains given object bool hasCollisionObject(const CollisionObjectPtr& object) const; @@ -83,6 +89,27 @@ class CollisionGroup /// Add collision objects to this CollisionGroup void addCollisionObjects(const CollisionObjectPtrs& objects); + /// Remove collision object from this CollisionGroup + void removeCollisionObject(const CollisionObjectPtr& object); + + /// Remove collision objects from this CollisionGroup + void removeCollisionObjects(const CollisionObjectPtrs& objects); + + /// Remove all the collision object in this CollisionGroup + void removeAllCollisionObjects(); + + /// Return array of collision objects + const CollisionObjectPtrs& getCollisionObjects(); + + /// Return array of (const) collision objects + const ConstCollisionObjectPtrs getCollisionObjects() const; + + /// Merge other CollisionGroup into this CollisionGroup + void unionGroup(const CollisionGroupPtr& other); + + /// Merge other CollisionGroup into this CollisionGroup + void subtractGroup(const CollisionGroupPtr& other); + /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); @@ -100,29 +127,27 @@ class CollisionGroup /// Return the collision detection engine specific data of this /// CollisionObject - CollisionGroupEngineData* getEngineData(); + CollisionGroupData* getEngineData(); /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void updateEngineData(); + // TODO(JS): remove protected: - /// Collision engine - EnginePtr mEngine; - // TODO(JS): Engine* ? + /// Collision detector + CollisionDetectorPtr mCollisionDetector; /// Collision objects CollisionObjectPtrs mCollisionObjects; /// Engine specific data - std::unique_ptr mEngineData; + std::unique_ptr mEngineData; }; // TODO(JS): make this class iterable for collision objects -using CollisionGroupPtr = std::shared_ptr; - } // namespace collision } // namespace dart diff --git a/dart/dynamics/CollisionGroup.cpp b/dart/collision/CollisionGroupData.cpp similarity index 80% rename from dart/dynamics/CollisionGroup.cpp rename to dart/collision/CollisionGroupData.cpp index 470b232d18994..ddca16e8b7ff9 100644 --- a/dart/dynamics/CollisionGroup.cpp +++ b/dart/collision/CollisionGroupData.cpp @@ -34,26 +34,25 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/dynamics/CollisionGroup.h" -#include "dart/dynamics/CollisionDetector.h" +#include "dart/collision/CollisionGroupData.h" + +#include namespace dart { -namespace dynamics { +namespace collision { //============================================================================== -CollisionGroup::CollisionGroup(const collision::EnginePtr& engine) - : collision::CollisionGroup(engine) +CollisionGroupData::CollisionGroupData(Engine* engine) + : mEngine(engine) { - + assert(mEngine); } //============================================================================== -void CollisionGroup::addCollisionObjects(const SkeletonPtr& skeleton) +const Engine* CollisionGroupData::getEngine() const { - auto objects = CollisionDetector::createCollisionObjects(mEngine, skeleton); - - collision::CollisionGroup::addCollisionObjects(objects); + return mEngine; } -} // namespace dynamics -} // namespace dart +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionGroupEngineData.h b/dart/collision/CollisionGroupData.h similarity index 86% rename from dart/collision/CollisionGroupEngineData.h rename to dart/collision/CollisionGroupData.h index 57593a01c0d68..0778ecf4e271e 100644 --- a/dart/collision/CollisionGroupEngineData.h +++ b/dart/collision/CollisionGroupData.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ -#define DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ +#ifndef DART_COLLISION_COLLISIONGROUPDATA_H_ +#define DART_COLLISION_COLLISIONGROUPDATA_H_ #include #include "dart/collision/SmartPointer.h" @@ -45,12 +45,14 @@ namespace collision { class CollisionObject; -class CollisionGroupEngineData +class CollisionGroupData { public: using CollisionObjectPtrs = std::vector; + CollisionGroupData(Engine* engine); + virtual void init() = 0; virtual void addCollisionObject(const CollisionObjectPtr& object, @@ -62,16 +64,24 @@ class CollisionGroupEngineData virtual void removeCollisionObject(const CollisionObjectPtr& object, bool init = true) = 0; + virtual void removeAllCollisionObjects(bool init = true) = 0; + /// Update engine data. This function will be called ahead of every collision /// checking virtual void update() = 0; - virtual std::unique_ptr clone( + virtual std::unique_ptr clone( const CollisionObjectPtrs& collObjects) const = 0; + const Engine* getEngine() const; + +protected: + + Engine* mEngine; + }; } // namespace collision } // namespace dart -#endif // DART_COLLISION_COLLISIONGROUPENGINEDATA_H_ +#endif // DART_COLLISION_COLLISIONGROUPDATA_H_ diff --git a/dart/collision/CollisionNode.h b/dart/collision/CollisionNode.h deleted file mode 100644 index ce90402b8d1cc..0000000000000 --- a/dart/collision/CollisionNode.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee , - * Tobias Kunz - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_COLLISIONNODE_H_ -#define DART_COLLISION_COLLISIONNODE_H_ - -#include - -namespace dart { -namespace dynamics { -class BodyNode; -} // namespace dynamics -} // namespace dart - -namespace dart { -namespace collision { - -/// \brief -class CollisionNode { -public: - /// \brief Default constructor - explicit CollisionNode(dynamics::BodyNode* _bodyNode); - - /// \brief Default destructor - virtual ~CollisionNode(); - - /// \brief - dynamics::BodyNode* getBodyNode() const; - - /// \brief - void setIndex(size_t _idx); - - /// \brief - size_t getIndex() const; - -protected: - /// \brief - dynamics::BodyNode* mBodyNode; - - /// \brief - size_t mIndex; -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_COLLISIONNODE_H_ diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 5212532b36fad..e8c459cd0a5fa 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -36,15 +36,16 @@ #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionObjectEngineData.h" +#include "dart/collision/CollisionDetector.h" +#include "dart/collision/CollisionObjectData.h" namespace dart { namespace collision { //============================================================================== -Engine* CollisionObject::getEngine() const +CollisionDetector* CollisionObject::getCollisionDetector() const { - return mEngine.get(); + return mCollisionDetector.get(); } //============================================================================== @@ -58,18 +59,18 @@ bool CollisionObject::detect(CollisionObject* other, const Option& option, Result& result) { - return mEngine->detect(this, other, option, result); + return mCollisionDetector->detect(this, other, option, result); } //============================================================================== bool CollisionObject::detect(CollisionGroup* group, const Option& option, Result& result) { - return mEngine->detect(this, group, option, result); + return mCollisionDetector->detect(this, group, option, result); } //============================================================================== -CollisionObjectEngineData* CollisionObject::getEngineData() const +CollisionObjectData* CollisionObject::getEngineData() const { return mEngineData.get(); } @@ -81,21 +82,32 @@ void CollisionObject::updateEngineData() } //============================================================================== -CollisionObject::CollisionObject(const EnginePtr& engine, - const dynamics::ShapePtr& shape) - : mEngine(engine), +CollisionObject::CollisionObject( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapePtr& shape) + : mCollisionDetector(collisionDetector), mShape(shape), - mEngineData(mEngine->createCollisionObjectData(this, mShape).release()) + mEngineData(mCollisionDetector->getEngine()->createCollisionObjectData( + this, mShape).release()) { - assert(mEngine); + assert(mCollisionDetector); assert(mShape); } //============================================================================== -FreeCollisionObject::FreeCollisionObject(const EnginePtr& engine, - const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf) - : CollisionObject(engine, shape), +std::shared_ptr FreeCollisionObject::create( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf) +{ + return std::make_shared(collisionDetector, shape, tf); +} + +//============================================================================== +FreeCollisionObject::FreeCollisionObject( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapePtr& shape, + const Eigen::Isometry3d& tf) + : CollisionObject(collisionDetector, shape), mW(tf) { // Do nothing diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index b45679ae56b1f..8f4ae1e7c6431 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -41,7 +41,7 @@ #include "dart/collision/Engine.h" #include "dart/collision/SmartPointer.h" -#include "dart/collision/CollisionObjectEngineData.h" +#include "dart/collision/CollisionObjectData.h" namespace dart { namespace collision { @@ -51,7 +51,7 @@ class CollisionObject public: /// Return collision detection engine associated with this CollisionObject - Engine* getEngine() const; + CollisionDetector* getCollisionDetector() const; /// Return shape pointer that associated with this CollisionObject dynamics::ShapePtr getShape() const; @@ -74,27 +74,42 @@ class CollisionObject /// Return the collision detection engine specific data of this /// CollisionObject - CollisionObjectEngineData* getEngineData() const; + CollisionObjectData* getEngineData() const; /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void updateEngineData(); + // TODO(JS): remove. instead, CollisionObjectData should be updated by engine + + template + T* as() + { + return static_cast(this); + } + + template + const T* as() const + { + return static_cast(this); + } + // TODO(JS): Need this? protected: /// Contructor - CollisionObject(const EnginePtr& engine, const dynamics::ShapePtr& shape); + CollisionObject(const CollisionDetectorPtr& engine, + const dynamics::ShapePtr& shape); protected: - /// Collision detection engine - EnginePtr mEngine; + /// Collision detector + CollisionDetectorPtr mCollisionDetector; /// Shape dynamics::ShapePtr mShape; /// Collision detection engine specific data - std::unique_ptr mEngineData; + std::unique_ptr mEngineData; }; @@ -104,8 +119,14 @@ class FreeCollisionObject : public CollisionObject { public: + static std::shared_ptr create( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapePtr& shape, + const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); + /// Constructor - FreeCollisionObject(const EnginePtr& engine, + FreeCollisionObject( + const CollisionDetectorPtr& collisionDetector, const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); // TODO(JS): change to engine pointer diff --git a/dart/dynamics/CollisionGroupGenerator.cpp b/dart/collision/CollisionObjectData.cpp similarity index 77% rename from dart/dynamics/CollisionGroupGenerator.cpp rename to dart/collision/CollisionObjectData.cpp index e8b11b5acc612..041834dffde88 100644 --- a/dart/dynamics/CollisionGroupGenerator.cpp +++ b/dart/collision/CollisionObjectData.cpp @@ -2,7 +2,7 @@ * Copyright (c) 2016, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Jeongseok Lee * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -34,23 +34,23 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/dynamics/CollisionGroupGenerator.h" +#include "dart/collision/CollisionObjectData.h" namespace dart { -namespace dynamics { +namespace collision { //============================================================================== -//collision::CollisionGroupPtr CollisionGroupGenerator::generate( -// const collision::CollisionDetectorPtr& cd, -// const SkeletonPtr& skeleton) -//{ -// // get list of collision node -// std::vector collisionNodes; +CollisionObjectData::CollisionObjectData(Engine* engine) + : mEngine(engine) +{ + // Do nothing +} -// collision::CollisionGroupPtr group = cd->createGroup(collisionNodes); - -// return group; -//} +//============================================================================== +const Engine* CollisionObjectData::getEngine() const +{ + return mEngine; +} -} // namespace dynamics -} // namespace dart +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionObjectEngineData.h b/dart/collision/CollisionObjectData.h similarity index 85% rename from dart/collision/CollisionObjectEngineData.h rename to dart/collision/CollisionObjectData.h index 3abe2a7833668..e33d1700fdf57 100644 --- a/dart/collision/CollisionObjectEngineData.h +++ b/dart/collision/CollisionObjectData.h @@ -34,20 +34,23 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_COLLISIONOBJECTENGINEDATA_H_ -#define DART_COLLISION_COLLISIONOBJECTENGINEDATA_H_ +#ifndef DART_COLLISION_COLLISIONOBJECTDATA_H_ +#define DART_COLLISION_COLLISIONOBJECTDATA_H_ #include +#include "dart/collision/SmartPointer.h" #include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { -class CollisionObjectEngineData +class CollisionObjectData { public: + CollisionObjectData(Engine* engine); + virtual void updateTransform(const Eigen::Isometry3d& tf) = 0; virtual void updateShape(const dynamics::ShapePtr& shape) = 0; @@ -55,10 +58,17 @@ class CollisionObjectEngineData /// Update engine data. This function will be called ahead of every collision /// checking virtual void update() = 0; + // TODO(JS): reorganize updateTansform/updateShape/update + + const Engine* getEngine() const; + +protected: + + Engine* mEngine; }; } // namespace collision } // namespace dart -#endif // DART_COLLISION_COLLISIONOBJECTENGINEDATA_H_ +#endif // DART_COLLISION_COLLISIONOBJECTDATA_H_ diff --git a/dart/collision/Contact.cpp b/dart/collision/Contact.cpp new file mode 100644 index 0000000000000..98f4269fbc0e8 --- /dev/null +++ b/dart/collision/Contact.cpp @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/Contact.h" + +namespace dart { +namespace collision { + +} // namespace collision +} // namespace dart diff --git a/dart/collision/Contact.h b/dart/collision/Contact.h new file mode 100644 index 0000000000000..53780f22776fa --- /dev/null +++ b/dart/collision/Contact.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_CONTACT_H_ +#define DART_COLLISION_CONTACT_H_ + +#include +#include "dart/collision/SmartPointer.h" +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace collision { + +/// Contact information +struct Contact +{ + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Contact point w.r.t. the world frame + Eigen::Vector3d point; + + /// Contact normal vector from bodyNode2 to bodyNode1 w.r.t. the world frame + Eigen::Vector3d normal; + + /// Contact force acting on bodyNode1 w.r.t. the world frame + /// + /// The contact force acting on bodyNode2 is -force, which is the opposite + /// direction of the force. + Eigen::Vector3d force; + + /// First colliding collision object + CollisionObject* collisionObject1; + + /// Second colliding collision object + CollisionObject* collisionObject2; + + /// Penetration depth + double penetrationDepth; + + // TODO(JS): triID1 will be deprecated when we don't use fcl_mesh + /// \brief + int triID1; + + // TODO(JS): triID2 will be deprecated when we don't use fcl_mesh + /// \brief + int triID2; + + // TODO(JS): userData is an experimental variable. + /// \brief User data. + void* userData; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_CONTACT_H_ diff --git a/dart/collision/Engine.cpp b/dart/collision/Engine.cpp index b52e869232eee..eb518b0d03112 100644 --- a/dart/collision/Engine.cpp +++ b/dart/collision/Engine.cpp @@ -39,16 +39,9 @@ namespace dart { namespace collision { -//============================================================================== -Option::Option(bool enableContact, size_t maxNumContacts) - : enableContact(enableContact), - maxNumContacts(maxNumContacts) -{ - // Do nothing -} - //============================================================================= -bool Engine::detect(CollisionGroup* group, CollisionObject* object, +bool Engine::detect(CollisionGroupData* group, + CollisionObjectData* object, const Option& option, Result& result) { return detect(object, group, option, result); diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h index 437da8f8b20fb..370d34ed92343 100644 --- a/dart/collision/Engine.h +++ b/dart/collision/Engine.h @@ -39,105 +39,58 @@ #include -#include "dart/collision/CollisionDetector.h" #include "dart/collision/SmartPointer.h" +#include "dart/collision/Option.h" +#include "dart/collision/Result.h" #include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { -struct Option -{ - /// Flag whether compute contact information such as point, normal, and - /// penetration depth. If this flag is set to false, the Engine returns only - /// simple information whether there is a collision of not. - bool enableContact; - - /// Maximum number of contacts to detect - size_t maxNumContacts; - - /// Constructor - Option(bool enableContact = true, - size_t maxNumContacts = 100); -}; - -struct Result -{ - /// List of contact information for each contact - std::vector contacts; -}; - class Engine { public: using CollisionObjectPtrs = std::vector; - /// Create a collision object - template - std::unique_ptr createCollisionObject( - const dynamics::ShapePtr& shape, - const Args&... args); - - /// Create a collision group - template - std::unique_ptr createCollisionGroup( - const CollisionObjectPtrs& collObjects, - const Args&... args); - /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; /// Create collision detection engine specific data for CollisionObject - virtual std::unique_ptr createCollisionObjectData( + virtual std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) = 0; /// Create collision detection engine specific data for CollisionGroup - virtual std::unique_ptr createCollisionGroupData( + virtual std::unique_ptr createCollisionGroupData( const CollisionObjectPtrs& collObjects) = 0; /// Perform collision detection for object1-object2. - virtual bool detect(CollisionObject* object1, CollisionObject* object2, + virtual bool detect(CollisionObjectData* object1, + CollisionObjectData* object2, const Option& option, Result& result) = 0; /// Perform collision detection for object-group. - virtual bool detect(CollisionObject* object, CollisionGroup* group, + virtual bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) = 0; /// Identical with detect(object, group, option, result) - bool detect(CollisionGroup* group, CollisionObject* object, + bool detect(CollisionGroupData* group, CollisionObjectData* object, const Option& option, Result& result); /// Perform collision detection for group. - virtual bool detect(CollisionGroup* group, + virtual bool detect(CollisionGroupData* group, const Option& option, Result& result) = 0; /// Perform collision detection for group1-group2. - virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, + virtual bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) = 0; }; -//============================================================================== -template -std::unique_ptr Engine::createCollisionObject( - const dynamics::ShapePtr& shape, const Args&... args) -{ - return std::unique_ptr( - new CollisionObjectType(this, shape, args...)); -} - -//============================================================================== -template -std::unique_ptr Engine::createCollisionGroup( - const Engine::CollisionObjectPtrs& collObjects, const Args&... args) -{ - return std::unique_ptr( - new CollisionGroupType(this, collObjects, args...)); -} - } // namespace collision } // namespace dart +#include "dart/collision/detail/Engine.h" + #endif // DART_COLLISION_ENGINE_H_ diff --git a/dart/collision/Option.cpp b/dart/collision/Option.cpp new file mode 100644 index 0000000000000..3649a451c511d --- /dev/null +++ b/dart/collision/Option.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/Option.h" + +namespace dart { +namespace collision { + +//============================================================================== +Option::Option(bool enableContact, size_t maxNumContacts) + : enableContact(enableContact), + maxNumContacts(maxNumContacts) +{ + // Do nothing +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/Option.h b/dart/collision/Option.h new file mode 100644 index 0000000000000..5d8835cc6463f --- /dev/null +++ b/dart/collision/Option.h @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_OPTION_H_ +#define DART_COLLISION_OPTION_H_ + +#include + +namespace dart { +namespace collision { + +struct Option +{ + /// Flag whether compute contact information such as point, normal, and + /// penetration depth. If this flag is set to false, the Engine returns only + /// simple information whether there is a collision of not. + bool enableContact; + + // TODO(JS): add option for immediet termination as soon as the first contact + // is detected + + /// Maximum number of contacts to detect + size_t maxNumContacts; + + /// Constructor + Option(bool enableContact = true, + size_t maxNumContacts = 100); +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_OPTION_H_ diff --git a/dart/collision/Result.cpp b/dart/collision/Result.cpp new file mode 100644 index 0000000000000..65a29807a5aee --- /dev/null +++ b/dart/collision/Result.cpp @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/Result.h" + +namespace dart { +namespace collision { + +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionNode.cpp b/dart/collision/Result.h similarity index 76% rename from dart/collision/CollisionNode.cpp rename to dart/collision/Result.h index 77a1fb78cbe3f..cdaeb3ad660a1 100644 --- a/dart/collision/CollisionNode.cpp +++ b/dart/collision/Result.h @@ -1,9 +1,8 @@ /* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation + * Copyright (c) 2016, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Jeongseok Lee , - * Tobias Kunz + * Author(s): Jeongseok Lee * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -35,29 +34,24 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/CollisionNode.h" +#ifndef DART_COLLISION_RESULT_H_ +#define DART_COLLISION_RESULT_H_ + +#include +#include "dart/collision/Contact.h" namespace dart { namespace collision { -CollisionNode::CollisionNode(dynamics::BodyNode* _bodyNode) - : mBodyNode(_bodyNode), mIndex(0) { -} - -CollisionNode::~CollisionNode() { -} - -dynamics::BodyNode*CollisionNode::getBodyNode() const { - return mBodyNode; -} +struct Result +{ + /// List of contact information for each contact + std::vector contacts; -void CollisionNode::setIndex(size_t _idx) { - mIndex = _idx; -} - -size_t CollisionNode::getIndex() const { - return mIndex; -} + size_t getNumContacts() const { return contacts.size(); } +}; } // namespace collision } // namespace dart + +#endif // DART_COLLISION_RESULT_H_ diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h index 09db3d0c682a2..08cb0ef611a56 100644 --- a/dart/collision/SmartPointer.h +++ b/dart/collision/SmartPointer.h @@ -43,6 +43,8 @@ namespace dart { namespace collision { +DART_COMMON_MAKE_SHARED_WEAK(CollisionDetector) + DART_COMMON_MAKE_SHARED_WEAK(Engine) DART_COMMON_MAKE_SHARED_WEAK(FCLEngine) DART_COMMON_MAKE_SHARED_WEAK(FCLMeshEngine) @@ -51,11 +53,12 @@ DART_COMMON_MAKE_SHARED_WEAK(FCLMeshEngine) #endif DART_COMMON_MAKE_SHARED_WEAK(CollisionObject) -DART_COMMON_MAKE_SHARED_WEAK(CollisionObjectEngineData) +DART_COMMON_MAKE_SHARED_WEAK(CollisionObjectData) DART_COMMON_MAKE_SHARED_WEAK(FreeCollisionObject) DART_COMMON_MAKE_SHARED_WEAK(CollisionGroup) -DART_COMMON_MAKE_SHARED_WEAK(CollisionGroupEngineData) +DART_COMMON_MAKE_SHARED_WEAK(CollisionGroupData) + } // namespace collision } // namespace dart diff --git a/dart/collision/detail/Engine.h b/dart/collision/detail/Engine.h new file mode 100644 index 0000000000000..33afc3f414e51 --- /dev/null +++ b/dart/collision/detail/Engine.h @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_DETAIL_ENGINE_H_ +#define DART_COLLISION_DETAIL_ENGINE_H_ + +#include "dart/collision/Engine.h" + +namespace dart { +namespace collision { + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_ENGINE_H_ diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp deleted file mode 100644 index 8bf02ed3026b6..0000000000000 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ /dev/null @@ -1,265 +0,0 @@ -/* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee , - * Tobias Kunz - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl/FCLCollisionDetector.h" - -#include - -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Skeleton.h" -#include "dart/collision/fcl/FCLCollisionNode.h" -#include "dart/collision/fcl/FCLTypes.h" - -#define FCL_VERSION_AT_LEAST(x,y,z) \ - (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \ - (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \ - FCL_PATCH_VERSION >= z)))) - -namespace dart { -namespace collision { - -//============================================================================== -// Collision data stores the collision request and the result given by -// collision algorithm. -struct FCLCollisionData -{ - FCLCollisionData() { done = false; } - - // Collision request - fcl::CollisionRequest request; - - // Collision result - fcl::CollisionResult result; - - // FCL collision detector - FCLCollisionDetector* collisionDetector; - - // Whether the collision iteration can stop - bool done; -}; - -//============================================================================== -bool collisionCallBack(fcl::CollisionObject* _o1, - fcl::CollisionObject* _o2, - void* _cdata) -{ - FCLCollisionData* cdata = static_cast(_cdata); - const fcl::CollisionRequest& request = cdata->request; - fcl::CollisionResult& result = cdata->result; - FCLCollisionDetector* cd = cdata->collisionDetector; - - if(cdata->done) - return true; - - // Filtering - if (!cd->isCollidable(cd->findCollisionNode(_o1), cd->findCollisionNode(_o2))) - return cdata->done; - - // Perform narrow-phase detection - fcl::collide(_o1, _o2, request, result); - - if (!request.enable_cost - && (result.isCollision()) - && (result.numContacts() >= request.num_max_contacts)) - { - cdata->done = true; - } - - return cdata->done; -} - -//============================================================================== -FCLCollisionDetector::FCLCollisionDetector() - : CollisionDetector(), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) -{ -} - -//============================================================================== -FCLCollisionDetector::~FCLCollisionDetector() -{ - delete mBroadPhaseAlg; -} - -//============================================================================== -CollisionNode* FCLCollisionDetector::createCollisionNode( - dynamics::BodyNode* _bodyNode) -{ - // This collision node will be removed at destructor of CollisionDetector. - FCLCollisionNode* collNode = new FCLCollisionNode(_bodyNode); - - for (size_t i = 0; i < collNode->getNumCollisionObjects(); ++i) - { - fcl::CollisionObject* collObj = collNode->getCollisionObject(i); - mBroadPhaseAlg->registerObject(collObj); - } - - // We do initial setup here for FCL's broad-phase algorithm since we don't - // have any other place to put this before performing collision detection - // frequently. - mBroadPhaseAlg->setup(); - - return collNode; -} - -//============================================================================== -bool isClose(const Eigen::Vector3d& _point1, const Eigen::Vector3d& _point2) -{ - if ((_point1 - _point2).squaredNorm() < 1e-12) - return true; - return false; -} - -//============================================================================== -bool hasClosePoint(const std::vector& _contacts, - const Eigen::Vector3d& _point) -{ - for (const auto& contact : _contacts) - { - if (isClose(contact.point, _point)) - return true; - } - - return false; -} - -//============================================================================== -bool FCLCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, - bool _calculateContactPoints) -{ - // TODO(JS): _checkAllCollisions - clearAllContacts(); - - // Set all the body nodes are not in colliding - for (size_t i = 0; i < mCollisionNodes.size(); i++) - mCollisionNodes[i]->getBodyNode()->setColliding(false); - - // Update all the transformations of the collision nodes - for (auto& collNode : mCollisionNodes) - static_cast(collNode)->updateFCLCollisionObjects(); - mBroadPhaseAlg->update(); - - FCLCollisionData collData; - collData.request.enable_contact = _calculateContactPoints; - // TODO: Uncomment below once we strict to use fcl 0.3.0 or greater - // collData.request.gjk_solver_type = fcl::GST_LIBCCD; - collData.request.num_max_contacts = getNumMaxContacts(); - collData.collisionDetector = this; - - // Perform broad-phase collision detection. Narrow-phase collision detection - // will be handled by the collision callback function. - mBroadPhaseAlg->collide(&collData, collisionCallBack); - - const size_t numContacts = collData.result.numContacts(); - for (size_t m = 0; m < numContacts; ++m) - { - const fcl::Contact& contact = collData.result.getContact(m); - - Eigen::Vector3d point = FCLTypes::convertVector3(contact.pos); - - if (hasClosePoint(mContacts, point)) - continue; - - Contact contactPair; - contactPair.point = point; - contactPair.normal = -FCLTypes::convertVector3(contact.normal); - contactPair.bodyNode1 = findCollisionNode(contact.o1)->getBodyNode(); - contactPair.bodyNode2 = findCollisionNode(contact.o2)->getBodyNode(); - contactPair.triID1 = contact.b1; - contactPair.triID2 = contact.b2; - contactPair.penetrationDepth = contact.penetration_depth; - assert(contactPair.bodyNode1.lock()); - assert(contactPair.bodyNode2.lock()); - - mContacts.push_back(contactPair); - } - - for (size_t i = 0; i < mContacts.size(); ++i) - { - // Set these two bodies are in colliding - mContacts[i].bodyNode1.lock()->setColliding(true); - mContacts[i].bodyNode2.lock()->setColliding(true); - } - - return !mContacts.empty(); -} - -//============================================================================== -bool FCLCollisionDetector::detectCollision(CollisionNode* /*_node1*/, - CollisionNode* /*_node2*/, - bool /*_calculateContactPoints*/) -{ - // TODO(JS): function not implemented - assert(false); - return false; -} - -//============================================================================== -CollisionNode* FCLCollisionDetector::findCollisionNode( - const fcl::CollisionGeometry* _fclCollGeom) const -{ - int numCollNodes = mCollisionNodes.size(); - for (int i = 0; i < numCollNodes; ++i) - { - FCLCollisionNode* collisionNode = - static_cast(mCollisionNodes[i]); - for (size_t j = 0; j < collisionNode->getNumCollisionObjects(); j++) - { -#if FCL_VERSION_AT_LEAST(0,3,0) - if (collisionNode->getCollisionObject(j)->collisionGeometry().get() - == _fclCollGeom) -#else - if (collisionNode->getCollisionObject(j)->getCollisionGeometry() - == _fclCollGeom) -#endif - return mCollisionNodes[i]; - } - } - return NULL; -} - -//============================================================================== -FCLCollisionNode* FCLCollisionDetector::findCollisionNode( - const fcl::CollisionObject* _fclCollObj) const -{ - FCLCollisionObjectUserData* userData = static_cast(_fclCollObj->getUserData()); - return userData->mFclCollNode; -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h deleted file mode 100644 index 2fa3387346ae8..0000000000000 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee , - * Tobias Kunz - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ - -#include -#include -#include - -#include "dart/collision/CollisionDetector.h" - -namespace dart { -namespace collision { - -class FCLCollisionNode; - -/// FCLCollisionDetector -class FCLCollisionDetector : public CollisionDetector -{ -public: - /// Constructor - FCLCollisionDetector(); - - /// Destructor - virtual ~FCLCollisionDetector(); - - // Documentation inherited - virtual bool detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints) override; - - // Documentation inherited - virtual CollisionNode* createCollisionNode(dynamics::BodyNode* _bodyNode) - override; - - /// Get collision node given FCL collision geometry - CollisionNode* findCollisionNode( - const fcl::CollisionGeometry* _fclCollGeom) const; - - /// Get collision node given FCL collision object - FCLCollisionNode* findCollisionNode( - const fcl::CollisionObject* _fclCollObj) const; - -protected: - // Documentation inherited - virtual bool detectCollision(CollisionNode* _node1, CollisionNode* _node2, - bool _calculateContactPoints) override; - - /// Broad-phase collision checker of FCL - fcl::DynamicAABBTreeCollisionManager* mBroadPhaseAlg; -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp b/dart/collision/fcl/FCLCollisionGroupData.cpp similarity index 70% rename from dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp rename to dart/collision/fcl/FCLCollisionGroupData.cpp index c6e1e3d6d4514..fca170d3d47a8 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.cpp +++ b/dart/collision/fcl/FCLCollisionGroupData.cpp @@ -34,22 +34,24 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h" +#include "dart/collision/fcl/FCLCollisionGroupData.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" namespace dart { namespace collision { //============================================================================== -FCLMeshCollisionGroupEngineData::FCLMeshCollisionGroupEngineData( - const FCLMeshCollisionGroupEngineData::CollisionObjects& collObjects) - : mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +FCLCollisionGroupData::FCLCollisionGroupData( + Engine* engine, + const FCLCollisionGroupData::CollisionObjects& collObjects) + : CollisionGroupData(engine), + mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { for (auto collObj : collObjects) { - auto data = static_cast(collObj->getEngineData()); + auto data = static_cast(collObj->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); } @@ -57,25 +59,25 @@ FCLMeshCollisionGroupEngineData::FCLMeshCollisionGroupEngineData( } //============================================================================== -std::unique_ptr -FCLMeshCollisionGroupEngineData::clone( - const CollisionGroupEngineData::CollisionObjectPtrs& collObjects) const +std::unique_ptr +FCLCollisionGroupData::clone(const CollisionObjectPtrs& collObjects) const { - return std::unique_ptr( - new FCLMeshCollisionGroupEngineData(collObjects)); + return std::unique_ptr( + new FCLCollisionGroupData(mEngine, collObjects)); } //============================================================================== -void FCLMeshCollisionGroupEngineData::init() +void FCLCollisionGroupData::init() { mBroadPhaseAlg->setup(); } //============================================================================== -void FCLMeshCollisionGroupEngineData::addCollisionObject( +void FCLCollisionGroupData::addCollisionObject( const CollisionObjectPtr& object, const bool init) { - auto data = static_cast(object->getEngineData()); + auto data = static_cast( + object->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); if (init) @@ -83,10 +85,11 @@ void FCLMeshCollisionGroupEngineData::addCollisionObject( } //============================================================================== -void FCLMeshCollisionGroupEngineData::removeCollisionObject( +void FCLCollisionGroupData::removeCollisionObject( const CollisionObjectPtr& object, const bool init) { - auto data = static_cast(object->getEngineData()); + auto data = static_cast( + object->getEngineData()); mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); if (init) @@ -94,14 +97,23 @@ void FCLMeshCollisionGroupEngineData::removeCollisionObject( } //============================================================================== -void FCLMeshCollisionGroupEngineData::update() +void FCLCollisionGroupData::removeAllCollisionObjects(bool init) +{ + mBroadPhaseAlg->clear(); + + if (init) + this->init(); +} + +//============================================================================== +void FCLCollisionGroupData::update() { mBroadPhaseAlg->update(); } //============================================================================== -FCLMeshCollisionGroupEngineData::FCLCollisionManager* -FCLMeshCollisionGroupEngineData::getFCLCollisionManager() const +FCLCollisionGroupData::FCLCollisionManager* +FCLCollisionGroupData::getFCLCollisionManager() const { return mBroadPhaseAlg.get(); } diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h b/dart/collision/fcl/FCLCollisionGroupData.h similarity index 85% rename from dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h rename to dart/collision/fcl/FCLCollisionGroupData.h index a8adcd78de0c0..ec430465db8d3 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h +++ b/dart/collision/fcl/FCLCollisionGroupData.h @@ -34,13 +34,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPENGINEDATA_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPENGINEDATA_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ #include #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupEngineData.h" +#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { @@ -48,7 +48,7 @@ namespace collision { class CollisionObject; class FCLCollisionObjectUserData; -class FCLMeshCollisionGroupEngineData : public CollisionGroupEngineData +class FCLCollisionGroupData : public CollisionGroupData { public: @@ -56,10 +56,10 @@ class FCLMeshCollisionGroupEngineData : public CollisionGroupEngineData using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLMeshCollisionGroupEngineData(const CollisionObjects& collObjects); + FCLCollisionGroupData(Engine* engine, const CollisionObjects& collObjects); // Documentation inherited - std::unique_ptr clone( + std::unique_ptr clone( const CollisionObjectPtrs& collObjects) const override; // Documentation inherited @@ -73,6 +73,9 @@ class FCLMeshCollisionGroupEngineData : public CollisionGroupEngineData void removeCollisionObject(const CollisionObjectPtr& object, const bool init) override; + // Documentation inherited + void removeAllCollisionObjects(bool init) override; + // Documentation inherited void update() override; @@ -89,4 +92,4 @@ class FCLMeshCollisionGroupEngineData : public CollisionGroupEngineData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPENGINEDATA_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ diff --git a/dart/collision/fcl/FCLCollisionNode.cpp b/dart/collision/fcl/FCLCollisionNode.cpp deleted file mode 100644 index 3fce0e9454d33..0000000000000 --- a/dart/collision/fcl/FCLCollisionNode.cpp +++ /dev/null @@ -1,725 +0,0 @@ -/* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl/FCLCollisionNode.h" - -#include -#include - -#include "dart/common/Console.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/MeshShape.h" -#include "dart/dynamics/SoftMeshShape.h" -#include "dart/collision/fcl/FCLTypes.h" - -#define FCL_VERSION_AT_LEAST(x,y,z) \ - (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \ - (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \ - FCL_PATCH_VERSION >= z)))) - -namespace dart { -namespace collision { - -//============================================================================== -// Create a cube mesh for collision detection -template -fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) -{ - int faces[6][4] = - { - {0, 1, 2, 3}, - {3, 2, 6, 7}, - {7, 6, 5, 4}, - {4, 5, 1, 0}, - {5, 6, 2, 1}, - {7, 4, 0, 3} - }; - float v[8][3]; - - v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; - v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; - v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; - v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; - v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; - v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 6; i++) - { - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); - p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - model->addTriangle(p1, p2, p3); - - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); - model->addTriangle(p1, p2, p3); - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) -{ - float v[59][3] = - { - {0, 0, 0}, - {0.135299, -0.461940, -0.135299}, - {0.000000, -0.461940, -0.191342}, - {-0.135299, -0.461940, -0.135299}, - {-0.191342, -0.461940, 0.000000}, - {-0.135299, -0.461940, 0.135299}, - {0.000000, -0.461940, 0.191342}, - {0.135299, -0.461940, 0.135299}, - {0.191342, -0.461940, 0.000000}, - {0.250000, -0.353553, -0.250000}, - {0.000000, -0.353553, -0.353553}, - {-0.250000, -0.353553, -0.250000}, - {-0.353553, -0.353553, 0.000000}, - {-0.250000, -0.353553, 0.250000}, - {0.000000, -0.353553, 0.353553}, - {0.250000, -0.353553, 0.250000}, - {0.353553, -0.353553, 0.000000}, - {0.326641, -0.191342, -0.326641}, - {0.000000, -0.191342, -0.461940}, - {-0.326641, -0.191342, -0.326641}, - {-0.461940, -0.191342, 0.000000}, - {-0.326641, -0.191342, 0.326641}, - {0.000000, -0.191342, 0.461940}, - {0.326641, -0.191342, 0.326641}, - {0.461940, -0.191342, 0.000000}, - {0.353553, 0.000000, -0.353553}, - {0.000000, 0.000000, -0.500000}, - {-0.353553, 0.000000, -0.353553}, - {-0.500000, 0.000000, 0.000000}, - {-0.353553, 0.000000, 0.353553}, - {0.000000, 0.000000, 0.500000}, - {0.353553, 0.000000, 0.353553}, - {0.500000, 0.000000, 0.000000}, - {0.326641, 0.191342, -0.326641}, - {0.000000, 0.191342, -0.461940}, - {-0.326641, 0.191342, -0.326641}, - {-0.461940, 0.191342, 0.000000}, - {-0.326641, 0.191342, 0.326641}, - {0.000000, 0.191342, 0.461940}, - {0.326641, 0.191342, 0.326641}, - {0.461940, 0.191342, 0.000000}, - {0.250000, 0.353553, -0.250000}, - {0.000000, 0.353553, -0.353553}, - {-0.250000, 0.353553, -0.250000}, - {-0.353553, 0.353553, 0.000000}, - {-0.250000, 0.353553, 0.250000}, - {0.000000, 0.353553, 0.353553}, - {0.250000, 0.353553, 0.250000}, - {0.353553, 0.353553, 0.000000}, - {0.135299, 0.461940, -0.135299}, - {0.000000, 0.461940, -0.191342}, - {-0.135299, 0.461940, -0.135299}, - {-0.191342, 0.461940, 0.000000}, - {-0.135299, 0.461940, 0.135299}, - {0.000000, 0.461940, 0.191342}, - {0.135299, 0.461940, 0.135299}, - {0.191342, 0.461940, 0.000000}, - {0.000000, -0.500000, 0.000000}, - {0.000000, 0.500000, 0.000000} - }; - - int f[112][3] = - { - {1, 2, 9}, - {9, 2, 10}, - {2, 3, 10}, - {10, 3, 11}, - {3, 4, 11}, - {11, 4, 12}, - {4, 5, 12}, - {12, 5, 13}, - {5, 6, 13}, - {13, 6, 14}, - {6, 7, 14}, - {14, 7, 15}, - {7, 8, 15}, - {15, 8, 16}, - {8, 1, 16}, - {16, 1, 9}, - {9, 10, 17}, - {17, 10, 18}, - {10, 11, 18}, - {18, 11, 19}, - {11, 12, 19}, - {19, 12, 20}, - {12, 13, 20}, - {20, 13, 21}, - {13, 14, 21}, - {21, 14, 22}, - {14, 15, 22}, - {22, 15, 23}, - {15, 16, 23}, - {23, 16, 24}, - {16, 9, 24}, - {24, 9, 17}, - {17, 18, 25}, - {25, 18, 26}, - {18, 19, 26}, - {26, 19, 27}, - {19, 20, 27}, - {27, 20, 28}, - {20, 21, 28}, - {28, 21, 29}, - {21, 22, 29}, - {29, 22, 30}, - {22, 23, 30}, - {30, 23, 31}, - {23, 24, 31}, - {31, 24, 32}, - {24, 17, 32}, - {32, 17, 25}, - {25, 26, 33}, - {33, 26, 34}, - {26, 27, 34}, - {34, 27, 35}, - {27, 28, 35}, - {35, 28, 36}, - {28, 29, 36}, - {36, 29, 37}, - {29, 30, 37}, - {37, 30, 38}, - {30, 31, 38}, - {38, 31, 39}, - {31, 32, 39}, - {39, 32, 40}, - {32, 25, 40}, - {40, 25, 33}, - {33, 34, 41}, - {41, 34, 42}, - {34, 35, 42}, - {42, 35, 43}, - {35, 36, 43}, - {43, 36, 44}, - {36, 37, 44}, - {44, 37, 45}, - {37, 38, 45}, - {45, 38, 46}, - {38, 39, 46}, - {46, 39, 47}, - {39, 40, 47}, - {47, 40, 48}, - {40, 33, 48}, - {48, 33, 41}, - {41, 42, 49}, - {49, 42, 50}, - {42, 43, 50}, - {50, 43, 51}, - {43, 44, 51}, - {51, 44, 52}, - {44, 45, 52}, - {52, 45, 53}, - {45, 46, 53}, - {53, 46, 54}, - {46, 47, 54}, - {54, 47, 55}, - {47, 48, 55}, - {55, 48, 56}, - {48, 41, 56}, - {56, 41, 49}, - {2, 1, 57}, - {3, 2, 57}, - {4, 3, 57}, - {5, 4, 57}, - {6, 5, 57}, - {7, 6, 57}, - {8, 7, 57}, - {1, 8, 57}, - {49, 50, 58}, - {50, 51, 58}, - {51, 52, 58}, - {52, 53, 58}, - {53, 54, 58}, - {54, 55, 58}, - {55, 56, 58}, - {56, 49, 58} - }; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 112; i++) - { - p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, - v[f[i][0]][1] * _sizeY, - v[f[i][0]][2] * _sizeZ); - p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, - v[f[i][1]][1] * _sizeY, - v[f[i][1]][2] * _sizeZ); - p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, - v[f[i][2]][1] * _sizeY, - v[f[i][2]][2] * _sizeZ); - - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - - return model; -} - -//============================================================================== -template -fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, - double _height, int _slices, int _stacks) -{ - const int CACHE_SIZE = 240; - - int i, j; - float sinCache[CACHE_SIZE]; - float cosCache[CACHE_SIZE]; - float angle; - float zBase; - float zLow, zHigh; - float sintemp, costemp; - float deltaRadius; - float radiusLow, radiusHigh; - - if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; - - if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || - _height < 0.0) - { - return nullptr; - } - - /* Center at CoM */ - zBase = -_height/2; - - /* Compute delta */ - deltaRadius = _baseRadius - _topRadius; - - /* Cache is the vertex locations cache */ - for (i = 0; i < _slices; i++) - { - angle = 2 * M_PI * i / _slices; - sinCache[i] = sin(angle); - cosCache[i] = cos(angle); - } - - sinCache[_slices] = sinCache[0]; - cosCache[_slices] = cosCache[0]; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3, p4; - - model->beginModel(); - - /* Base of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _baseRadius; - zLow = zBase; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - /* Body of cylinder */ - for (i = 0; i < _slices; i++) - { - for (j = 0; j < _stacks; j++) - { - zLow = j * _height / _stacks + zBase; - zHigh = (j + 1) * _height / _stacks + zBase; - radiusLow = _baseRadius - - deltaRadius * (static_cast(j) / _stacks); - radiusHigh = _baseRadius - - deltaRadius * (static_cast(j + 1) / _stacks); - - p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], - zLow); - p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], - zLow); - p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], - zHigh); - p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], - zHigh); - - model->addTriangle(p1, p2, p3); - model->addTriangle(p2, p3, p4); - } - } - - /* Top of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _topRadius; - zLow = zBase + _height; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, - const aiScene* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - for (size_t i = 0; i < _mesh->mNumMeshes; i++) - { - for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) - { - fcl::Vec3f vertices[3]; - for (size_t k = 0; k < 3; k++) - { - const aiVector3D& vertex - = _mesh->mMeshes[i]->mVertices[ - _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x * _scaleX, - vertex.y * _scaleY, - vertex.z * _scaleZ); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - - for (size_t i = 0; i < _mesh->mNumFaces; i++) - { - fcl::Vec3f vertices[3]; - for (size_t j = 0; j < 3; j++) - { - const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - - model->endModel(); - return model; -} - -//============================================================================== -FCLCollisionObjectUserData::FCLCollisionObjectUserData( - FCLCollisionNode* fclCollNode, - dynamics::BodyNode* bodyNode, - const dynamics::ShapePtr& shape) - : mFclCollNode(fclCollNode), - mBodyNode(bodyNode), - mShape(shape) -{ - // Do nothing -} - -//============================================================================== -FCLCollisionObjectUserData::FCLCollisionObjectUserData( - CollisionObject* collisionObject, - const dynamics::ShapePtr& shape) - : mCollisionObject(collisionObject), - mShape(shape) -{ - // Do nothing -} - -//============================================================================== -FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) - : CollisionNode(_bodyNode) -{ - using dynamics::Shape; - using dynamics::BoxShape; - using dynamics::EllipsoidShape; - using dynamics::CylinderShape; - using dynamics::PlaneShape; - using dynamics::MeshShape; - using dynamics::SoftMeshShape; - - for (size_t i = 0; i < _bodyNode->getNumCollisionShapes(); i++) - { - dynamics::ShapePtr shape = _bodyNode->getCollisionShape(i); - - boost::shared_ptr fclCollGeom; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - const BoxShape* box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset(createCube(size[0], size[1], size[2])); -#else - fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); -#endif - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - EllipsoidShape* ellipsoid = static_cast(shape.get()); - - const Eigen::Vector3d& size = ellipsoid->getSize(); - - if (ellipsoid->isSphere()) - { - fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); - } - else - { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset( - createEllipsoid(ellipsoid->getSize()[0], - ellipsoid->getSize()[1], - ellipsoid->getSize()[2])); -#else - fclCollGeom.reset( - new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); -#endif - } - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - const CylinderShape* cylinder - = static_cast(shape.get()); - const double radius = cylinder->getRadius(); - const double height = cylinder->getHeight(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); -#else - fclCollGeom.reset(new fcl::Cylinder(radius, height)); -#endif - // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 - // returns single contact point for cylinder yet. - break; - } - case Shape::PLANE: - { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); -#else - assert(dynamic_cast(shape.get())); - dynamics::PlaneShape* plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - fclCollGeom.reset( - new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); -#endif - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - MeshShape* shapeMesh = static_cast(shape.get()); - fclCollGeom.reset( - createMesh(shapeMesh->getScale()[0], - shapeMesh->getScale()[1], - shapeMesh->getScale()[2], - shapeMesh->getMesh())); - - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - SoftMeshShape* softMeshShape = static_cast(shape.get()); - fclCollGeom.reset( - createSoftMesh(softMeshShape->getAssimpMesh())); - - break; - } - default: - { - dterr << "[FCLCollisionNode::FCLCollisionNode] Attempting to create " - << "unsupported shape type '" << shape->getShapeType() << "' of '" - << _bodyNode->getName() << "' body node." << std::endl; - continue; - } - } - - assert(nullptr != fclCollGeom); - fcl::CollisionObject* fclCollObj - = new fcl::CollisionObject(fclCollGeom, getFCLTransform(i)); - fclCollGeom->setUserData(new FCLCollisionObjectUserData(this, _bodyNode, shape)); - mCollisionObjects.push_back(fclCollObj); - } -} - -//============================================================================== -FCLCollisionNode::~FCLCollisionNode() -{ - for (const auto& collObj : mCollisionObjects) - { - assert(collObj); - - delete static_cast(collObj->getUserData()); - delete collObj; - } -} - -//============================================================================== -size_t FCLCollisionNode::getNumCollisionObjects() const -{ - return mCollisionObjects.size(); -} - -//============================================================================== -const FCLCollisionNode::FCLCollisionObjects& -FCLCollisionNode::getCollisionObjects() -{ - return mCollisionObjects; -} - -//============================================================================== -fcl::CollisionObject* FCLCollisionNode::getCollisionObject(size_t _idx) const -{ - assert(_idx < mCollisionObjects.size()); - return mCollisionObjects[_idx]; -} - -//============================================================================== -fcl::Transform3f FCLCollisionNode::getFCLTransform(size_t _idx) const -{ - Eigen::Isometry3d worldTrans - = mBodyNode->getTransform() - * mBodyNode->getCollisionShape(_idx)->getLocalTransform(); - - return fcl::Transform3f( - fcl::Matrix3f(worldTrans(0, 0), worldTrans(0, 1), worldTrans(0, 2), - worldTrans(1, 0), worldTrans(1, 1), worldTrans(1, 2), - worldTrans(2, 0), worldTrans(2, 1), worldTrans(2, 2)), - fcl::Vec3f(worldTrans(0, 3), worldTrans(1, 3), worldTrans(2, 3))); -} - -//============================================================================== -void FCLCollisionNode::updateFCLCollisionObjects() -{ - using dart::dynamics::BodyNode; - using dart::dynamics::Shape; - using dart::dynamics::SoftMeshShape; - - for (auto& fclCollObj : mCollisionObjects) - { - FCLCollisionObjectUserData* userData - = static_cast(fclCollObj->collisionGeometry()->getUserData()); - - BodyNode* bodyNode = userData->mBodyNode; - Shape* shape = userData->mShape.get(); - - // Update soft-body's vertices - if (shape->getShapeType() == Shape::SOFT_MESH) - { - assert(dynamic_cast(shape)); - SoftMeshShape* softMeshShape = static_cast(shape); - - const aiMesh* mesh = softMeshShape->getAssimpMesh(); - softMeshShape->update(); -#if FCL_VERSION_AT_LEAST(0,3,0) - fcl::CollisionGeometry* collGeom - = const_cast( - fclCollObj->collisionGeometry().get()); -#else - fcl::CollisionGeometry* collGeom - = const_cast( - fclCollObj->getCollisionGeometry()); -#endif - assert(nullptr != dynamic_cast*>(collGeom)); - fcl::BVHModel* bvhModel - = static_cast*>(collGeom); - - bvhModel->beginUpdateModel(); - for (size_t j = 0; j < mesh->mNumFaces; j++) - { - fcl::Vec3f vertices[3]; - for (size_t k = 0; k < 3; k++) - { - const aiVector3D& vertex - = mesh->mVertices[mesh->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - } - bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]); - } - bvhModel->endUpdateModel(); - } - - // Update shape's transform - const Eigen::Isometry3d W = bodyNode->getWorldTransform() - * shape->getLocalTransform(); - fclCollObj->setTransform(FCLTypes::convertTransform(W)); - fclCollObj->computeAABB(); - } -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl/FCLCollisionNode.h b/dart/collision/fcl/FCLCollisionNode.h deleted file mode 100644 index 733e0e9113621..0000000000000 --- a/dart/collision/fcl/FCLCollisionNode.h +++ /dev/null @@ -1,133 +0,0 @@ -/* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCL_FCLCOLLISIONNODE_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONNODE_H_ - -#include - -#include -#include -#include -#include - -#include "dart/collision/CollisionNode.h" -#include "dart/collision/SmartPointer.h" -#include "dart/dynamics/Shape.h" - -namespace dart { -namespace dynamics { -class BodyNode; -class Shape; -} // namespace dynamics -} // namespace dart - -namespace dart { -namespace collision { - -class FCLCollisionNode; -class CollisionObject; - -struct FCLCollisionObjectUserData -{ - FCLCollisionNode* mFclCollNode; - CollisionObject* mCollisionObject; - dynamics::BodyNode* mBodyNode; - dynamics::ShapePtr mShape; - - FCLCollisionObjectUserData(FCLCollisionNode* mFclCollNode, - dynamics::BodyNode* mBodyNode, - const dynamics::ShapePtr& mShape); - - FCLCollisionObjectUserData(CollisionObject* mFclCollNode, - const dynamics::ShapePtr& mShape); - -}; - -class ConstFCLCollisionObjects -{ -public: - using ConstIterator = std::vector::const_iterator; - - ConstFCLCollisionObjects(const std::vector& container) - : mContainer(container) - {} - - ConstIterator begin() const { return mContainer.cbegin(); } - ConstIterator end() const; - -protected: - std::vector mContainer; -}; - -/// FCLCollisionNode -class FCLCollisionNode : public CollisionNode -{ -public: - - using FCLCollisionObjects = std::vector; - - /// Constructor - explicit FCLCollisionNode(dynamics::BodyNode* _bodyNode); - - /// Destructor - virtual ~FCLCollisionNode(); - - /// Get number of collision objects - size_t getNumCollisionObjects() const; - - const FCLCollisionObjects& getCollisionObjects(); - - /// Get FCL collision object given index - fcl::CollisionObject* getCollisionObject(size_t _idx) const; - - /// Get FCL transformation of shape given index - fcl::Transform3f getFCLTransform(size_t _idx) const; - - /// Update transformation and AABB of all the fcl collision objects. - void updateFCLCollisionObjects(); - -private: - - /// Array of FCL collision object that continas geometry and transform - FCLCollisionObjects mCollisionObjects; - -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_FCL_FCLCOLLISIONNODE_H_ diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.cpp b/dart/collision/fcl/FCLCollisionObjectData.cpp similarity index 96% rename from dart/collision/fcl/FCLCollisionObjectEngineData.cpp rename to dart/collision/fcl/FCLCollisionObjectData.cpp index 183367ba5b703..1a02574d5783b 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectData.cpp @@ -34,17 +34,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLCollisionObjectEngineData.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" #include #include #include #include "dart/common/Console.h" -#include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/Engine.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" @@ -596,11 +596,22 @@ boost::shared_ptr createFCLCollisionGeometry( } // anonymous namespace + +//============================================================================== +FCLCollisionObjectUserData::FCLCollisionObjectUserData( + CollisionObject* collisionObject, + const dynamics::ShapePtr& shape) + : mCollisionObject(collisionObject), + mShape(shape) +{ + // Do nothing +} + //============================================================================== -FCLCollisionObjectEngineData::FCLCollisionObjectEngineData( +FCLCollisionObjectData::FCLCollisionObjectData(Engine* engine, CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectEngineData(), + : CollisionObjectData(engine), mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), mFCLCollisionObject(new fcl::CollisionObject( createFCLCollisionGeometry(shape))) @@ -609,14 +620,14 @@ FCLCollisionObjectEngineData::FCLCollisionObjectEngineData( } //============================================================================== -void FCLCollisionObjectEngineData::updateTransform(const Eigen::Isometry3d& tf) +void FCLCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) { mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); mFCLCollisionObject->computeAABB(); } //============================================================================== -void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) +void FCLCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) { auto fclCollGeom = createFCLCollisionGeometry(shape); @@ -624,7 +635,7 @@ void FCLCollisionObjectEngineData::updateShape(const dynamics::ShapePtr& shape) } //============================================================================== -void FCLCollisionObjectEngineData::update() +void FCLCollisionObjectData::update() { using dart::dynamics::BodyNode; using dart::dynamics::Shape; @@ -673,7 +684,7 @@ void FCLCollisionObjectEngineData::update() } //============================================================================== -fcl::CollisionObject* FCLCollisionObjectEngineData::getFCLCollisionObject() const +fcl::CollisionObject* FCLCollisionObjectData::getFCLCollisionObject() const { return mFCLCollisionObject.get(); } diff --git a/dart/collision/fcl/FCLCollisionObjectEngineData.h b/dart/collision/fcl/FCLCollisionObjectData.h similarity index 79% rename from dart/collision/fcl/FCLCollisionObjectEngineData.h rename to dart/collision/fcl/FCLCollisionObjectData.h index 9a7727d19132e..c7065f7b2ac1e 100644 --- a/dart/collision/fcl/FCLCollisionObjectEngineData.h +++ b/dart/collision/fcl/FCLCollisionObjectData.h @@ -34,29 +34,40 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECTENGINEDATA_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONOBJECTENGINEDATA_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ #include #include #include -#include "dart/collision/CollisionObjectEngineData.h" +#include "dart/collision/CollisionObjectData.h" namespace dart { namespace collision { class CollisionObject; -class FCLCollisionObjectUserData; -class FCLCollisionObjectEngineData : public CollisionObjectEngineData +struct FCLCollisionObjectUserData +{ + CollisionObject* mCollisionObject; + dynamics::BodyNode* mBodyNode; + dynamics::ShapePtr mShape; + + FCLCollisionObjectUserData(CollisionObject* mFclCollNode, + const dynamics::ShapePtr& mShape); + +}; + +class FCLCollisionObjectData : public CollisionObjectData { public: /// Constructor - FCLCollisionObjectEngineData(CollisionObject* parent, - const dynamics::ShapePtr& shape); + FCLCollisionObjectData(Engine* engine, + CollisionObject* parent, + const dynamics::ShapePtr& shape); // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; @@ -86,4 +97,4 @@ class FCLCollisionObjectEngineData : public CollisionObjectEngineData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECTENGINEDATA_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index 0dfc6a390917b..467e5490165de 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -43,9 +43,8 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionNode.h" -#include "dart/collision/fcl/FCLCollisionObjectEngineData.h" -#include "dart/collision/fcl/FCLCollisionGroupEngineData.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionGroupData.h" namespace dart { namespace collision { @@ -104,15 +103,21 @@ struct FCLCollisionData //============================================================================== -const std::string& FCLEngine::getType() const +FCLEnginePtr FCLEngine::create() { - return getTypeStatic(); + return FCLEnginePtr(new FCLEngine()); } //============================================================================== -FCLEnginePtr FCLEngine::create() +FCLEngine::FCLEngine() { - return FCLEnginePtr(new FCLEngine()); + // Do nothing +} + +//============================================================================== +FCLEngine::~FCLEngine() +{ + // Do nothing } //============================================================================== @@ -123,41 +128,44 @@ const std::string& FCLEngine::getTypeStatic() } //============================================================================== -std::unique_ptr FCLEngine::createCollisionObjectData( +const std::string& FCLEngine::getType() const +{ + return getTypeStatic(); +} + +//============================================================================== +std::unique_ptr FCLEngine::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { - return std::unique_ptr( - new FCLCollisionObjectEngineData(parent, shape)); + return std::unique_ptr( + new FCLCollisionObjectData(this, parent, shape)); } //============================================================================== -std::unique_ptr FCLEngine::createCollisionGroupData( +std::unique_ptr FCLEngine::createCollisionGroupData( const CollisionObjectPtrs& collObjects) { - return std::unique_ptr( - new FCLCollisionGroupEngineData(collObjects)); + return std::unique_ptr( + new FCLCollisionGroupData(this, collObjects)); } //============================================================================== -bool FCLEngine::detect(CollisionObject* object1, - CollisionObject* object2, - const Option& option, - Result& result) +bool FCLEngine::detect( + CollisionObjectData* objectData1, + CollisionObjectData* objectData2, + const Option& option, Result& result) { result.contacts.clear(); - assert(object1->getEngine()->getType() == FCLEngine::getTypeStatic()); - assert(object2->getEngine()->getType() == FCLEngine::getTypeStatic()); - - object1->updateEngineData(); - object2->updateEngineData(); + assert(objectData1->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(objectData2->getEngine()->getType() == FCLEngine::getTypeStatic()); - auto data1 = static_cast(object1->getEngineData()); - auto data2 = static_cast(object2->getEngineData()); + auto castedData1 = static_cast(objectData1); + auto castedData2 = static_cast(objectData2); - auto fclCollObj1 = data1->getFCLCollisionObject(); - auto fclCollObj2 = data2->getFCLCollisionObject(); + auto fclCollObj1 = castedData1->getFCLCollisionObject(); + auto fclCollObj2 = castedData2->getFCLCollisionObject(); FCLCollisionData collData(&option, &result); collisionCallBack(fclCollObj1, fclCollObj2, &collData); @@ -166,24 +174,23 @@ bool FCLEngine::detect(CollisionObject* object1, } //============================================================================== -bool FCLEngine::detect(CollisionObject* object, CollisionGroup* group, - const Option& option, Result& result) +bool FCLEngine::detect( + CollisionObjectData* objectData, + CollisionGroupData* groupData, + const Option& option, Result& result) { result.contacts.clear(); - assert(object); - assert(group); - assert(object->getEngine()->getType() == FCLEngine::getTypeStatic()); - assert(group->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(objectData); + assert(groupData); + assert(objectData->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(groupData->getEngine()->getType() == FCLEngine::getTypeStatic()); - object->updateEngineData(); - group->updateEngineData(); + auto castedObjData = static_cast(objectData); + auto castedGrpData = static_cast(groupData); - auto objData = static_cast(object->getEngineData()); - auto groupData = static_cast(group->getEngineData()); - - auto fclObject = objData->getFCLCollisionObject(); - auto broadPhaseAlg = groupData->getFCLCollisionManager(); + auto fclObject = castedObjData->getFCLCollisionObject(); + auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); @@ -192,19 +199,17 @@ bool FCLEngine::detect(CollisionObject* object, CollisionGroup* group, } //============================================================================== -bool FCLEngine::detect(CollisionGroup* group, +bool FCLEngine::detect(CollisionGroupData* groupData, const Option& option, Result& result) { result.contacts.clear(); - assert(group); - assert(group->getEngine()->getType() == FCLEngine::getTypeStatic()); - - group->updateEngineData(); + assert(groupData); + assert(groupData->getEngine()->getType() == FCLEngine::getTypeStatic()); - auto data = static_cast(group->getEngineData()); + auto castedData = static_cast(groupData); - auto broadPhaseAlg = data->getFCLCollisionManager(); + auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(&collData, collisionCallBack); @@ -213,24 +218,22 @@ bool FCLEngine::detect(CollisionGroup* group, } //============================================================================== -bool FCLEngine::detect(CollisionGroup* group1, CollisionGroup* group2, +bool FCLEngine::detect(CollisionGroupData* groupData1, + CollisionGroupData* groupData2, const Option& option, Result& result) { result.contacts.clear(); - assert(group1); - assert(group2); - assert(group1->getEngine()->getType() == FCLEngine::getTypeStatic()); - assert(group2->getEngine()->getType() == FCLEngine::getTypeStatic()); - - group1->updateEngineData(); - group2->updateEngineData(); + assert(groupData1); + assert(groupData2); + assert(groupData1->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(groupData2->getEngine()->getType() == FCLEngine::getTypeStatic()); - auto data1 = static_cast(group1->getEngineData()); - auto data2 = static_cast(group2->getEngineData()); + auto castedData1 = static_cast(groupData1); + auto castedData2 = static_cast(groupData2); - auto broadPhaseAlg1 = data1->getFCLCollisionManager(); - auto broadPhaseAlg2 = data2->getFCLCollisionManager(); + auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); + auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); @@ -263,6 +266,9 @@ bool collisionCallBack(fcl::CollisionObject* o1, // return collData->done; // TODO(JS): disabled until other functionalities are implemented + // Clear previous results + fclResult.clear(); + // Perform narrow-phase detection fcl::collide(o1, o2, fclRequest, fclResult); @@ -287,11 +293,92 @@ void postProcess(const fcl::CollisionResult& fclResult, Result& result) { auto numContacts = fclResult.numContacts(); + + if (0 == numContacts) + return; + + const double ZERO = 0.000001; + const double ZERO2 = ZERO*ZERO; + + std::vector markForDeletion(numContacts, false); + + // mark all the repeated points + for (auto i = 0u; i < numContacts - 1; ++i) + { + const auto& contact1 = fclResult.getContact(i); + + for (auto j = i + 1u; j < numContacts; ++j) + { + const auto& contact2 = fclResult.getContact(j); + + const auto diff = contact1.pos - contact2.pos; + + if (diff.length() < 3 * ZERO2) + { + markForDeletion[i] = true; + break; + } + } + } + + // remove all the co-linear contact points for (auto i = 0u; i < numContacts; ++i) { - const auto fclContact = fclResult.getContact(i); - result.contacts.push_back(convertContact(fclContact, o1, o2)); + if (markForDeletion[i]) + continue; + + const auto& contact1 = fclResult.getContact(i); + + for (auto j = i + 1u; j < numContacts; ++j) + { + if (markForDeletion[j]) + continue; + + const auto& contact2 = fclResult.getContact(j); + + for (auto k = j + 1u; k < numContacts; ++k) + { + if (markForDeletion[k]) + continue; + + const auto& contact3 = fclResult.getContact(k); + + const auto va = contact1.pos - contact2.pos; + const auto vb = contact1.pos - contact3.pos; + const auto v = va.cross(vb); + + if (v.length() < ZERO2) + { + markForDeletion[i] = true; + break; + } + } + } + } + + for (size_t i = 0; i < numContacts; ++i) + { + if (!markForDeletion[i]) + { + const auto fclContact = fclResult.getContact(i); + result.contacts.push_back(convertContact(fclContact, o1, o2)); + } + } + + std::cout << "=================" << std::endl; + std::cout << "# of contacts:" << result.contacts.size() << std::endl; + std::cout << "=================" << std::endl; + for (auto i = 0u; i < result.contacts.size(); ++i) + { + auto contact = result.contacts[i]; + + std::cout << "Contact (" << i << ")" << std::endl; + std::cout << "Point : " << contact.point.transpose() << std::endl; + std::cout << "Normal: " << contact.normal.transpose() << std::endl; + std::cout << "Depth : " << contact.penetrationDepth << std::endl; + std::cout << std::endl; } + std::cout << "=================" << std::endl; } //============================================================================== @@ -325,8 +412,6 @@ Contact convertContact(const fcl::Contact& fclContact, = static_cast(o2->getUserData()); assert(userData1); assert(userData2); - contact.shape1 = userData1->mShape; - contact.shape2 = userData2->mShape; contact.collisionObject1 = userData1->mCollisionObject; contact.collisionObject2 = userData2->mCollisionObject; diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index 4ff01437ba815..9bbec26cbdc4a 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -53,6 +53,12 @@ class FCLEngine : public Engine static FCLEnginePtr create(); + /// Constructor + FCLEngine(); + + /// Constructor + virtual ~FCLEngine(); + /// Return engine type "FCL" static const std::string& getTypeStatic(); @@ -60,34 +66,30 @@ class FCLEngine : public Engine const std::string& getType() const override; // Documentation inherit - std::unique_ptr createCollisionObjectData( + std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; // Documentation inherit - std::unique_ptr createCollisionGroupData( + std::unique_ptr createCollisionGroupData( const CollisionObjectPtrs& collObjects) override; // Documentation inherit - bool detect(CollisionObject* object1, CollisionObject* object2, + bool detect(CollisionObjectData* object1, CollisionObjectData* object2, const Option& option, Result& result) override; // Documentation inherit - bool detect(CollisionObject* object, CollisionGroup* group, + bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) override; // Documentation inherit - bool detect(CollisionGroup* group, + bool detect(CollisionGroupData* group, const Option& option, Result& result) override; // Documentation inherit - bool detect(CollisionGroup* group1, CollisionGroup* group2, + bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; -protected: - - FCLEngine() = default; - }; } // namespace collision diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp deleted file mode 100644 index 8a4cc4c500ab4..0000000000000 --- a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Chen Tang , - * Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" - -#include -#include - -#include - -#include "dart/renderer/LoadOpengl.h" -#include "dart/math/Helpers.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/SoftBodyNode.h" -#include "dart/dynamics/PointMass.h" -#include "dart/collision/CollisionNode.h" -#include "dart/collision/fcl_mesh/CollisionShapes.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionNode.h" - -namespace dart { -namespace collision { - -//============================================================================== -FCLMeshCollisionDetector::FCLMeshCollisionDetector() -{ -} - -//============================================================================== -FCLMeshCollisionDetector::~FCLMeshCollisionDetector() -{ -} - -//============================================================================== -CollisionNode*FCLMeshCollisionDetector::createCollisionNode( - dynamics::BodyNode* _bodyNode) -{ - return new FCLMeshCollisionNode(_bodyNode); -} - -//============================================================================== -bool FCLMeshCollisionDetector::detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints) -{ - //---------------------------------------------------------------------------- - // Clear previous contact informations - //---------------------------------------------------------------------------- - - // Update the positions of vertices on meshs - for (size_t i = 0; i < mCollisionNodes.size(); ++i) - static_cast(mCollisionNodes[i])->updateShape(); - - // Clear previous contacts - mContacts.clear(); - - //---------------------------------------------------------------------------- - // Detect collisions - //---------------------------------------------------------------------------- - - bool collision = false; - - FCLMeshCollisionNode* FCLMeshCollisionNode1 = nullptr; - FCLMeshCollisionNode* FCLMeshCollisionNode2 = nullptr; - - for (size_t i = 0; i < mCollisionNodes.size(); i++) - { - FCLMeshCollisionNode1 - = static_cast(mCollisionNodes[i]); - for (size_t j = i + 1; j < mCollisionNodes.size(); j++) - { - FCLMeshCollisionNode2 - = static_cast(mCollisionNodes[j]); - if (!isCollidable(FCLMeshCollisionNode1, FCLMeshCollisionNode2)) - continue; - - std::vector* contactPoints - = _calculateContactPoints ? &mContacts : nullptr; - if (FCLMeshCollisionNode1->detectCollision(FCLMeshCollisionNode2, - contactPoints, - mNumMaxContacts)) - { - collision = true; - mCollisionNodes[i]->getBodyNode()->setColliding(true); - mCollisionNodes[j]->getBodyNode()->setColliding(true); - - if (!_checkAllCollisions) - return true; - } - } - } - - return collision; -} - -//============================================================================== -bool FCLMeshCollisionDetector::detectCollision(CollisionNode* _node1, - CollisionNode* _node2, - bool _calculateContactPoints) -{ - FCLMeshCollisionNode* collisionNode1 = - static_cast(_node1); - FCLMeshCollisionNode* collisionNode2 = - static_cast(_node2); - return collisionNode1->detectCollision( - collisionNode2, - _calculateContactPoints ? &mContacts : nullptr, - mNumMaxContacts); -} - -//============================================================================== -void FCLMeshCollisionDetector::draw() -{ - for (size_t i = 0; i < mCollisionNodes.size(); i++) - { - static_cast( - mCollisionNodes[i])->drawCollisionSkeletonNode(); - } -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h deleted file mode 100644 index 9b84564b20b56..0000000000000 --- a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Chen Tang , - * Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCL_MESH_FCLMESHCOLLISIONDETECTOR_H_ -#define DART_COLLISION_FCL_MESH_FCLMESHCOLLISIONDETECTOR_H_ - -#include "dart/collision/CollisionDetector.h" - -namespace dart { - -namespace dynamics { -class SoftBodyNode; -class PointMass; -} // namespace dynamics - -namespace collision { - -/// -class FCLMeshCollisionDetector : public CollisionDetector -{ -public: - /// Constructor - FCLMeshCollisionDetector(); - - /// Destructor - virtual ~FCLMeshCollisionDetector(); - - // Documentation inherited - virtual CollisionNode* createCollisionNode(dynamics::BodyNode* _bodyNode); - - // Documentation inherited - virtual bool detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints); - - // Documentation inherited - virtual bool detectCollision(CollisionNode* _node1, CollisionNode* _node2, - bool _calculateContactPoints); - - /// - void draw(); -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_FCL_MESH_FCLMESHCOLLISIONDETECTOR_H_ - diff --git a/dart/collision/fcl/FCLCollisionGroupEngineData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp similarity index 68% rename from dart/collision/fcl/FCLCollisionGroupEngineData.cpp rename to dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp index 2232ace7e2fce..4119ff18184c0 100644 --- a/dart/collision/fcl/FCLCollisionGroupEngineData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp @@ -34,22 +34,24 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLCollisionGroupEngineData.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLCollisionObjectEngineData.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" namespace dart { namespace collision { //============================================================================== -FCLCollisionGroupEngineData::FCLCollisionGroupEngineData( - const FCLCollisionGroupEngineData::CollisionObjects& collObjects) - : mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +FCLMeshCollisionGroupData::FCLMeshCollisionGroupData( + Engine* engine, + const FCLMeshCollisionGroupData::CollisionObjects& collObjects) + : CollisionGroupData(engine), + mBroadPhaseAlg(new FCLCollisionManager()) { for (auto collObj : collObjects) { - auto data = static_cast(collObj->getEngineData()); + auto data = static_cast(collObj->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); } @@ -57,24 +59,26 @@ FCLCollisionGroupEngineData::FCLCollisionGroupEngineData( } //============================================================================== -std::unique_ptr -FCLCollisionGroupEngineData::clone(const CollisionObjectPtrs& collObjects) const +std::unique_ptr +FCLMeshCollisionGroupData::clone( + const CollisionGroupData::CollisionObjectPtrs& collObjects) const { - return std::unique_ptr( - new FCLCollisionGroupEngineData(collObjects)); + return std::unique_ptr( + new FCLMeshCollisionGroupData(mEngine, collObjects)); } //============================================================================== -void FCLCollisionGroupEngineData::init() +void FCLMeshCollisionGroupData::init() { mBroadPhaseAlg->setup(); } //============================================================================== -void FCLCollisionGroupEngineData::addCollisionObject( +void FCLMeshCollisionGroupData::addCollisionObject( const CollisionObjectPtr& object, const bool init) { - auto data = static_cast(object->getEngineData()); + auto data = static_cast( + object->getEngineData()); mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); if (init) @@ -82,10 +86,11 @@ void FCLCollisionGroupEngineData::addCollisionObject( } //============================================================================== -void FCLCollisionGroupEngineData::removeCollisionObject( +void FCLMeshCollisionGroupData::removeCollisionObject( const CollisionObjectPtr& object, const bool init) { - auto data = static_cast(object->getEngineData()); + auto data = static_cast( + object->getEngineData()); mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); if (init) @@ -93,14 +98,24 @@ void FCLCollisionGroupEngineData::removeCollisionObject( } //============================================================================== -void FCLCollisionGroupEngineData::update() +void FCLMeshCollisionGroupData::removeAllCollisionObjects(bool init) { + mBroadPhaseAlg->clear(); + + if (init) + this->init(); +} + +//============================================================================== +void FCLMeshCollisionGroupData::update() +{ + mBroadPhaseAlg->setup(); mBroadPhaseAlg->update(); } //============================================================================== -FCLCollisionGroupEngineData::FCLCollisionManager* -FCLCollisionGroupEngineData::getFCLCollisionManager() const +FCLMeshCollisionGroupData::FCLCollisionManager* +FCLMeshCollisionGroupData::getFCLCollisionManager() const { return mBroadPhaseAlg.get(); } diff --git a/dart/collision/fcl/FCLCollisionGroupEngineData.h b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h similarity index 80% rename from dart/collision/fcl/FCLCollisionGroupEngineData.h rename to dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h index a80acf3ba6dc0..9bbf9309c0fb7 100644 --- a/dart/collision/fcl/FCLCollisionGroupEngineData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h @@ -34,13 +34,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUPENGINEDATA_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONGROUPENGINEDATA_H_ +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPDATA_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPDATA_H_ #include +#include #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupEngineData.h" +#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { @@ -48,18 +49,20 @@ namespace collision { class CollisionObject; class FCLCollisionObjectUserData; -class FCLCollisionGroupEngineData : public CollisionGroupEngineData +class FCLMeshCollisionGroupData : public CollisionGroupData { public: - using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; +// using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; + using FCLCollisionManager = fcl::NaiveCollisionManager; using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLCollisionGroupEngineData(const CollisionObjects& collObjects); + FCLMeshCollisionGroupData(Engine* engine, + const CollisionObjects& collObjects); // Documentation inherited - std::unique_ptr clone( + std::unique_ptr clone( const CollisionObjectPtrs& collObjects) const override; // Documentation inherited @@ -73,6 +76,9 @@ class FCLCollisionGroupEngineData : public CollisionGroupEngineData void removeCollisionObject(const CollisionObjectPtr& object, const bool init) override; + // Documentation inherited + void removeAllCollisionObjects(bool init) override; + // Documentation inherited void update() override; @@ -89,4 +95,4 @@ class FCLCollisionGroupEngineData : public CollisionGroupEngineData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUPENGINEDATA_H_ +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPDATA_H_ diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp deleted file mode 100644 index 8c1a71d83c934..0000000000000 --- a/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp +++ /dev/null @@ -1,482 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Chen Tang , - * Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl_mesh/FCLMeshCollisionNode.h" - -#include -#include - -#include -#include -#include - -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/MeshShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/SoftMeshShape.h" -#include "dart/renderer/LoadOpengl.h" -#include "dart/collision/fcl_mesh/CollisionShapes.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" - -namespace dart { -namespace collision { - -//============================================================================== -FCLMeshCollisionNode::FCLMeshCollisionNode(dynamics::BodyNode* _bodyNode) - : CollisionNode(_bodyNode) -{ - // using-declaration - using dart::dynamics::Shape; - using dart::dynamics::BoxShape; - using dart::dynamics::EllipsoidShape; - using dart::dynamics::CylinderShape; - using dart::dynamics::MeshShape; - using dart::dynamics::SoftMeshShape; - - // Create meshes according to types of the shapes - for (size_t i = 0; i < _bodyNode->getNumCollisionShapes(); i++) - { - dynamics::ShapePtr shape = _bodyNode->getCollisionShape(i); - fcl::Transform3f shapeT = getFclTransform(shape->getLocalTransform()); - switch (shape->getShapeType()) - { - case Shape::ELLIPSOID: - { - EllipsoidShape* ellipsoid = static_cast(shape.get()); - // Sphere - if (ellipsoid->isSphere()) - { - fcl::BVHModel* mesh = new fcl::BVHModel; - fcl::generateBVHModel( - *mesh, fcl::Sphere(ellipsoid->getSize()[0]*0.5), shapeT, 10, 10); - mMeshes.push_back(mesh); - // Ellipsoid - } - else - { - mMeshes.push_back(createEllipsoid( - ellipsoid->getSize()[0], ellipsoid->getSize()[1], - ellipsoid->getSize()[2], shapeT)); - } - break; - } - case dynamics::Shape::BOX: - { - BoxShape* box = static_cast(shape.get()); - mMeshes.push_back(createCube( - box->getSize()[0], box->getSize()[1], box->getSize()[2], shapeT)); - break; - } - case dynamics::Shape::CYLINDER: - { - CylinderShape* cylinder = static_cast(shape.get()); - double radius = cylinder->getRadius(); - double height = cylinder->getHeight(); - mMeshes.push_back(createCylinder( - radius, radius, height, 16, 16, shapeT)); - break; - } - case dynamics::Shape::MESH: - { - MeshShape* shapeMesh = static_cast(shape.get()); - mMeshes.push_back(createMesh(shapeMesh->getScale()[0], - shapeMesh->getScale()[1], - shapeMesh->getScale()[2], - shapeMesh->getMesh(), - shapeT)); - break; - } - case dynamics::Shape::SOFT_MESH: - { - SoftMeshShape* softMeshShape = static_cast(shape.get()); - mMeshes.push_back(createSoftMesh( - softMeshShape->getAssimpMesh(), shapeT)); - break; - } - default: - { - std::cout << "ERROR: Collision checking does not support " - << _bodyNode->getName() << "'s Shape type\n"; - break; - } - } - } -} - -//============================================================================== -FCLMeshCollisionNode::~FCLMeshCollisionNode() -{ - for (size_t i = 0; i < mMeshes.size(); i++) - delete mMeshes[i]; -} - -//============================================================================== -bool FCLMeshCollisionNode::detectCollision(FCLMeshCollisionNode* _otherNode, - std::vector* _contactPoints, - int _num_max_contact) -{ - evalRT(); - _otherNode->evalRT(); - bool collision = false; - - for (size_t i = 0; i < mMeshes.size(); i++) - { - for (size_t j = 0; j < _otherNode->mMeshes.size(); j++) - { - fcl::CollisionResult res; - fcl::CollisionRequest req; - - // only evaluate contact points if data structure for returning the - // contact points was provided - req.enable_contact = _contactPoints; - req.num_max_contacts = _num_max_contact; - fcl::collide(mMeshes[i], - mFclWorldTrans, - _otherNode->mMeshes[j], - _otherNode->mFclWorldTrans, - req, res); - - if (res.isCollision()) - collision = true; - - if (!_contactPoints) - { - if (collision) - return true; - else - continue; - } - - - int numCoplanarContacts = 0; - int numNoContacts = 0; - int numContacts = 0; - - std::vector unfilteredContactPoints; - unfilteredContactPoints.reserve(res.numContacts()); - - for (size_t k = 0; k < res.numContacts(); k++) - { - // for each pair of intersecting triangles, we create two contact points - Contact pair1, pair2; - // pair1.bd1 = mBodyNode; - // pair1.bd2 = _otherNode->mBodyNode; - // pair1.bdID1 = this->mBodyNodeID; - // pair1.bdID2 = _otherNode->mBodyNodeID; - pair1.bodyNode1 = this->getBodyNode(); - pair1.bodyNode2 = _otherNode->getBodyNode(); - fcl::Vec3f v; - pair1.triID1 = res.getContact(k).b1; - pair1.triID2 = res.getContact(k).b2; - pair1.penetrationDepth = res.getContact(k).penetration_depth; - pair1.shape1 = pair1.bodyNode1.lock()->getCollisionShape(i); - pair1.shape2 = pair1.bodyNode2.lock()->getCollisionShape(j); - pair2 = pair1; - int contactResult = - evalContactPosition(res.getContact(k), mMeshes[i], - _otherNode->mMeshes[j], - mFclWorldTrans, _otherNode->mFclWorldTrans, - &pair1.point, &pair2.point); - if (contactResult == COPLANAR_CONTACT) - { - numCoplanarContacts++; - // if (numContacts != 0 || numCoplanarContacts > 1) - if (numContacts > 2) - continue; - } - else if (contactResult == NO_CONTACT) - { - numNoContacts++; - continue; - } - else - { - numContacts++; - } - - v = -res.getContact(k).normal; - pair1.normal = Eigen::Vector3d(v[0], v[1], v[2]); - pair2.normal = Eigen::Vector3d(v[0], v[1], v[2]); - - unfilteredContactPoints.push_back(pair1); - unfilteredContactPoints.push_back(pair2); - } - - const double ZERO = 0.000001; - const double ZERO2 = ZERO*ZERO; - - std::vector markForDeletion(unfilteredContactPoints.size(), false); - - // mark all the repeated points - for (unsigned int k = 0; k < unfilteredContactPoints.size(); k++) - { - for (unsigned int l = k + 1; l < unfilteredContactPoints.size(); l++) - { - Eigen::Vector3d diff = unfilteredContactPoints[k].point - - unfilteredContactPoints[l].point; - if (diff.dot(diff) < 3 * ZERO2) - { - markForDeletion[k] = true; - break; - } - } - } - - // remove all the co-linear contact points - for (size_t k = 0; k < unfilteredContactPoints.size(); k++) - { - if (markForDeletion[k]) - continue; - for (size_t l = 0; l < unfilteredContactPoints.size(); l++) - { - if (l == k || markForDeletion[l]) - continue; - if (markForDeletion[k]) - break; - for (size_t m = l + 1; m < unfilteredContactPoints.size(); m++) - { - if (k == m) - continue; - Eigen::Vector3d v = - (unfilteredContactPoints[k].point - - unfilteredContactPoints[l].point).cross( - unfilteredContactPoints[k].point - - unfilteredContactPoints[m].point); - if (v.dot(v) < ZERO2 - && ((unfilteredContactPoints[k].point - - unfilteredContactPoints[l].point).dot( - unfilteredContactPoints[k].point - - unfilteredContactPoints[m].point) < 0)) { - markForDeletion[k] = true; - break; - } - } - } - } - - for (size_t k = 0; k < unfilteredContactPoints.size(); k++) - { - if (!markForDeletion[k]) - _contactPoints->push_back(unfilteredContactPoints[k]); - } - } - } - return collision; -} - -//============================================================================== -void FCLMeshCollisionNode::updateShape() -{ - // using-declaration - using dart::dynamics::SoftMeshShape; - - for (size_t i = 0; i < mBodyNode->getNumCollisionShapes(); i++) - { - dynamics::ShapePtr shape = mBodyNode->getCollisionShape(i); - fcl::Transform3f shapeT = getFclTransform(shape->getLocalTransform()); - switch (shape->getShapeType()) - { - case dynamics::Shape::SOFT_MESH: - { - SoftMeshShape* softMeshShape = static_cast(shape.get()); - const aiMesh* mesh = softMeshShape->getAssimpMesh(); - softMeshShape->update(); - - mMeshes[i]->beginUpdateModel(); - - for (unsigned int j = 0; j < mesh->mNumFaces; j++) - { - fcl::Vec3f vertices[3]; - for (unsigned int k = 0; k < 3; k++) - { - const aiVector3D& vertex - = mesh->mVertices[mesh->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - vertices[k] = shapeT.transform(vertices[k]); - } - mMeshes[i]->updateTriangle(vertices[0], vertices[1], vertices[2]); - } - - mMeshes[i]->endUpdateModel(); - break; - } - default: - { - break; - } - } - } -} - -//============================================================================== -void FCLMeshCollisionNode::evalRT() -{ - mWorldTrans = mBodyNode->getTransform(); - mFclWorldTrans = getFclTransform(mWorldTrans); -} - -//============================================================================== -fcl::Transform3f FCLMeshCollisionNode::getFclTransform( - const Eigen::Isometry3d& _m) -{ - return fcl::Transform3f(fcl::Matrix3f(_m(0, 0), _m(0, 1), _m(0, 2), - _m(1, 0), _m(1, 1), _m(1, 2), - _m(2, 0), _m(2, 1), _m(2, 2)), - fcl::Vec3f(_m(0, 3), _m(1, 3), _m(2, 3))); -} - -//============================================================================== -int FCLMeshCollisionNode::evalContactPosition( - const fcl::Contact& _fclContact, - fcl::BVHModel* _mesh1, - fcl::BVHModel* _mesh2, - const fcl::Transform3f& _transform1, - const fcl::Transform3f& _transform2, - Eigen::Vector3d* _contactPosition1, - Eigen::Vector3d* _contactPosition2) -{ - int id1 = _fclContact.b1; - int id2 = _fclContact.b2; - fcl::Triangle tri1 = _mesh1->tri_indices[id1]; - fcl::Triangle tri2 = _mesh2->tri_indices[id2]; - - fcl::Vec3f v1, v2, v3, p1, p2, p3; - v1 = _mesh1->vertices[tri1[0]]; - v2 = _mesh1->vertices[tri1[1]]; - v3 = _mesh1->vertices[tri1[2]]; - - p1 = _mesh2->vertices[tri2[0]]; - p2 = _mesh2->vertices[tri2[1]]; - p3 = _mesh2->vertices[tri2[2]]; - - fcl::Vec3f contact1, contact2; - v1 = _transform1.transform(v1); - v2 = _transform1.transform(v2); - v3 = _transform1.transform(v3); - p1 = _transform2.transform(p1); - p2 = _transform2.transform(p2); - p3 = _transform2.transform(p3); - int testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); - - if (testRes == COPLANAR_CONTACT) - { - double area1 = triArea(v1, v2, v3); - double area2 = triArea(p1, p2, p3); -// std::cout << "this node = " << this->mBodynodeID << " other node = " -// << _other->mBodynodeID << std::endl; - if (area1 < area2) - contact1 = v1 + v2 + v3; - else - contact1 = p1 + p2 + p3; - contact1[0] /= 3.0; - contact1[1] /= 3.0; - contact1[2] /= 3.0; - contact2 = contact1; -// std::cout << contact1[0] << " " << contact1[1] << " " -// << contact1[2] << std::endl; - } - *_contactPosition1 = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); - *_contactPosition2 = Eigen::Vector3d(contact2[0], contact2[1], contact2[2]); - return testRes; -} - -//============================================================================== -void FCLMeshCollisionNode::drawCollisionSkeletonNode(bool _bTrans) -{ - evalRT(); - double M[16]; - for (int i = 0; i < 4; i++) - for (int j = 0; j < 4; j++) - M[j * 4 + i] = mWorldTrans(i, j); -// fcl::Vec3f v1, v2, v3; - glPushMatrix(); - if (_bTrans) - glMultMatrixd(M); - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - glBegin(GL_TRIANGLES); - for (size_t i = 0; i < mMeshes.size(); i++) - { - for (int j = 0; j < mMeshes[i]->num_tris; j++) - { - fcl::Triangle tri = mMeshes[i]->tri_indices[j]; - glVertex3f(mMeshes[i]->vertices[tri[0]][0], - mMeshes[i]->vertices[tri[0]][1], - mMeshes[i]->vertices[tri[0]][2]); - glVertex3f(mMeshes[i]->vertices[tri[1]][0], - mMeshes[i]->vertices[tri[1]][1], - mMeshes[i]->vertices[tri[1]][2]); - glVertex3f(mMeshes[i]->vertices[tri[2]][0], - mMeshes[i]->vertices[tri[2]][1], - mMeshes[i]->vertices[tri[2]][2]); - } - } - glEnd(); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - glPopMatrix(); -} - -//============================================================================== -template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh, - const fcl::Transform3f& _transform) -{ - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - - for (unsigned int i = 0; i < _mesh->mNumFaces; i++) - { - fcl::Vec3f vertices[3]; - for (unsigned int j = 0; j < 3; j++) - { - const aiVector3D& vertex - = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - vertices[j] = _transform.transform(vertices[j]); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - - model->endModel(); - return model; -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionNode.h b/dart/collision/fcl_mesh/FCLMeshCollisionNode.h deleted file mode 100644 index 87d528684410b..0000000000000 --- a/dart/collision/fcl_mesh/FCLMeshCollisionNode.h +++ /dev/null @@ -1,244 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Chen Tang , - * Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCLMESH_FCLMESHCOLLISIONNODE_H_ -#define DART_COLLISION_FCLMESH_FCLMESHCOLLISIONNODE_H_ - -#include - -#include -#include -#include -#include - -#include "dart/collision/CollisionNode.h" -#include "dart/collision/CollisionDetector.h" -#include "dart/collision/fcl_mesh/tri_tri_intersection_test.h" - -namespace dart { -namespace dynamics { -class BodyNode; -} // namespace dynamics -} // namespace dart - -namespace dart { -namespace collision { - -/// class FCLMeshCollisionNode -class FCLMeshCollisionNode : public CollisionNode -{ -public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// Constructor - explicit FCLMeshCollisionNode(dynamics::BodyNode* _bodyNode); - - /// Destructor - virtual ~FCLMeshCollisionNode(); - - /// - std::vector*> mMeshes; - - /// - fcl::Transform3f mFclWorldTrans; - - /// - Eigen::Isometry3d mWorldTrans; - - /// - virtual bool detectCollision(FCLMeshCollisionNode* _otherNode, - std::vector* _contactPoints, - int _max_num_contact); - /// - void updateShape(); - - /// - void evalRT(); - - /// - static fcl::Transform3f getFclTransform(const Eigen::Isometry3d& _m); - - /// - static int evalContactPosition(const fcl::Contact& _fclContact, - fcl::BVHModel* _mesh1, - fcl::BVHModel* _mesh2, - const fcl::Transform3f& _transform1, - const fcl::Transform3f& _transform2, - Eigen::Vector3d* _contactPosition1, - Eigen::Vector3d* _contactPosition2); - - /// - void drawCollisionSkeletonNode(bool _bTrans = true); - -private: - /// - static int FFtest( - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, - fcl::Vec3f* res1, fcl::Vec3f* res2); - - /// - bool EFtest(const fcl::Vec3f& p0, const fcl::Vec3f& p1, - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - fcl::Vec3f* p); - - /// - static double triArea(fcl::Vec3f p1, fcl::Vec3f p2, fcl::Vec3f p3); -}; - -/// -template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh, - const fcl::Transform3f& _transform); - -//============================================================================== -inline bool FCLMeshCollisionNode::EFtest(const fcl::Vec3f& p0, - const fcl::Vec3f& p1, - const fcl::Vec3f& r1, - const fcl::Vec3f& r2, - const fcl::Vec3f& r3, - fcl::Vec3f* p) -{ - double ZERO1 = 0.00000001; - fcl::Vec3f n = (r3 - r1).cross(r2 - r1); - double s = (p1 - p0).dot(n); - if (std::abs(s) < ZERO1) return false; - double t = (r1 - p0).dot(n) / s; - fcl::Vec3f tmp = p0 + (p1 - p0) * t; - if (t >= 0 - && t <= 1 - && (((tmp - r1).cross(r2 - r1)).dot(n) >= 0) - && (((tmp - r2).cross(r3 - r2)).dot(n) >= 0) - && (((tmp - r3).cross(r1 - r3)).dot(n) >= 0) - ) - { - *p = tmp; - return true; - } - return false; -} - -//============================================================================== -inline int FCLMeshCollisionNode::FFtest( - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, - fcl::Vec3f* res1, fcl::Vec3f* res2) -{ - float U0[3], U1[3], U2[3], V0[3], V1[3], V2[3], RES1[3], RES2[3]; - SET(U0, r1); - SET(U1, r2); - SET(U2, r3); - SET(V0, R1); - SET(V1, R2); - SET(V2, R3); - - int contactResult = tri_tri_intersect(V0, V1, V2, U0, U1, U2, RES1, RES2); - - SET((*res1), RES1); - SET((*res2), RES2); - return contactResult; - /* - int count1 = 0, count2 = 0, count = 0; - //fcl::Vec3f p1 = fcl::Vec3f(0, 0, 0), p2 = fcl::Vec3f(0, 0, 0); - fcl::Vec3f tmp; - fcl::Vec3f p[6], p1[4], p2[4]; - - if(EFtest(r1, r2, R1, R2, R3, tmp)) - { - p[count] = tmp; - count++; - } - if(EFtest(r2, r3, R1, R2, R3, tmp)) - { - p[count] = tmp; - count++; - } - if(EFtest(r3, r1, R1, R2, R3, tmp)) - { - p[count] = tmp; - count++; - } - - if(EFtest(R1, R2, r1, r2, r3, tmp)) - { - p[count] = tmp; - count++; - } - if(EFtest(R2, R3, r1, r2, r3, tmp)) - { - p[count] = tmp; - count++; - } - if(EFtest(R3, R1, r1, r2, r3, tmp)) - { - p[count] = tmp; - count++; - } - if(count==0) return false; - else - { - - res1 = fcl::Vec3f(100000, 1000000, 1000000); - res2 = -res1; - for (int i=0; i #include #include +#include #include "dart/common/Console.h" -#include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/Engine.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLCollisionNode.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" @@ -489,7 +490,12 @@ boost::shared_ptr createFCLCollisionGeometry( assert(dynamic_cast(shape.get())); const BoxShape* box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); + + auto model = new fcl::BVHModel; + fcl::generateBVHModel(*model, fcl::Box(size[0], size[1], size[2]), fcl::Transform3f()); + fclCollGeom.reset(createCube(size[0], size[1], size[2])); +// fclCollGeom.reset(model); break; } case Shape::ELLIPSOID: @@ -556,10 +562,11 @@ boost::shared_ptr createFCLCollisionGeometry( } // anonymous namespace //============================================================================== -FCLMeshCollisionObjectEngineData::FCLMeshCollisionObjectEngineData( +FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( + Engine* engine, CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectEngineData(), + : CollisionObjectData(engine), mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), mFCLCollisionObject(new fcl::CollisionObject( createFCLCollisionGeometry(shape))) @@ -568,7 +575,7 @@ FCLMeshCollisionObjectEngineData::FCLMeshCollisionObjectEngineData( } //============================================================================== -void FCLMeshCollisionObjectEngineData::updateTransform( +void FCLMeshCollisionObjectData::updateTransform( const Eigen::Isometry3d& tf) { mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); @@ -576,7 +583,7 @@ void FCLMeshCollisionObjectEngineData::updateTransform( } //============================================================================== -void FCLMeshCollisionObjectEngineData::updateShape( +void FCLMeshCollisionObjectData::updateShape( const dynamics::ShapePtr& shape) { auto fclCollGeom = createFCLCollisionGeometry(shape); @@ -586,7 +593,7 @@ void FCLMeshCollisionObjectEngineData::updateShape( } //============================================================================== -void FCLMeshCollisionObjectEngineData::update() +void FCLMeshCollisionObjectData::update() { using dart::dynamics::BodyNode; using dart::dynamics::Shape; @@ -637,7 +644,7 @@ void FCLMeshCollisionObjectEngineData::update() } //============================================================================== -fcl::CollisionObject* FCLMeshCollisionObjectEngineData::getFCLCollisionObject() const +fcl::CollisionObject* FCLMeshCollisionObjectData::getFCLCollisionObject() const { return mFCLCollisionObject.get(); } diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h similarity index 84% rename from dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h rename to dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h index 7f96d9e178133..70e89b97a20ec 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h @@ -34,15 +34,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTENGINEDATA_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTENGINEDATA_H_ +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTDATA_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTDATA_H_ #include #include #include -#include "dart/collision/CollisionObjectEngineData.h" +#include "dart/collision/CollisionObjectData.h" namespace dart { namespace collision { @@ -50,13 +50,14 @@ namespace collision { class CollisionObject; class FCLCollisionObjectUserData; -class FCLMeshCollisionObjectEngineData : public CollisionObjectEngineData +class FCLMeshCollisionObjectData : public CollisionObjectData { public: /// Constructor - FCLMeshCollisionObjectEngineData(CollisionObject* parent, - const dynamics::ShapePtr& shape); + FCLMeshCollisionObjectData(Engine* engine, + CollisionObject* parent, + const dynamics::ShapePtr& shape); // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; @@ -83,4 +84,4 @@ class FCLMeshCollisionObjectEngineData : public CollisionObjectEngineData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTENGINEDATA_H_ +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTDATA_H_ diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.cpp b/dart/collision/fcl_mesh/FCLMeshEngine.cpp index f1e4e3e0884e3..129a868350e3c 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.cpp +++ b/dart/collision/fcl_mesh/FCLMeshEngine.cpp @@ -44,10 +44,10 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionNode.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionNode.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectEngineData.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupEngineData.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl_mesh/tri_tri_intersection_test.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h" namespace dart { namespace collision { @@ -63,6 +63,10 @@ void postProcess(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o2, Result& result); +Contact convertContact(const fcl::Contact& fclContact, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2); + void convertOption(const Option& option, fcl::CollisionRequest& fclRequest); /// Collision data stores the collision request and the result given by @@ -118,15 +122,27 @@ double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); //============================================================================== -const std::string& FCLMeshEngine::getType() const +FCLMeshEnginePtr FCLMeshEngine::create() { - return getTypeStatic(); + return FCLMeshEnginePtr(new FCLMeshEngine()); } //============================================================================== -FCLMeshEnginePtr FCLMeshEngine::create() +FCLMeshEngine::FCLMeshEngine() { - return FCLMeshEnginePtr(new FCLMeshEngine()); + // Do nothing +} + +//============================================================================== +FCLMeshEngine::~FCLMeshEngine() +{ + // Do nothing +} + +//============================================================================== +const std::string& FCLMeshEngine::getType() const +{ + return getTypeStatic(); } //============================================================================== @@ -137,43 +153,40 @@ const std::string& FCLMeshEngine::getTypeStatic() } //============================================================================== -std::unique_ptr +std::unique_ptr FCLMeshEngine::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { - return std::unique_ptr( - new FCLMeshCollisionObjectEngineData(parent, shape)); + return std::unique_ptr( + new FCLMeshCollisionObjectData(this, parent, shape)); } //============================================================================== -std::unique_ptr +std::unique_ptr FCLMeshEngine::createCollisionGroupData( const CollisionObjectPtrs& collObjects) { - return std::unique_ptr( - new FCLMeshCollisionGroupEngineData(collObjects)); + return std::unique_ptr( + new FCLMeshCollisionGroupData(this, collObjects)); } //============================================================================== -bool FCLMeshEngine::detect(CollisionObject* object1, - CollisionObject* object2, - const Option& option, - Result& result) +bool FCLMeshEngine::detect( + CollisionObjectData* objectData1, + CollisionObjectData* objectData2, + const Option& option, Result& result) { result.contacts.clear(); - assert(object1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - assert(object2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(objectData1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(objectData2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - object1->updateEngineData(); - object2->updateEngineData(); + auto castedData1 = static_cast(objectData1); + auto castedData2 = static_cast(objectData2); - auto data1 = static_cast(object1->getEngineData()); - auto data2 = static_cast(object2->getEngineData()); - - auto fclCollObj1 = data1->getFCLCollisionObject(); - auto fclCollObj2 = data2->getFCLCollisionObject(); + auto fclCollObj1 = castedData1->getFCLCollisionObject(); + auto fclCollObj2 = castedData2->getFCLCollisionObject(); FCLCollisionData collData(&option, &result); collisionCallBack(fclCollObj1, fclCollObj2, &collData); @@ -182,24 +195,23 @@ bool FCLMeshEngine::detect(CollisionObject* object1, } //============================================================================== -bool FCLMeshEngine::detect(CollisionObject* object, CollisionGroup* group, - const Option& option, Result& result) +bool FCLMeshEngine::detect( + CollisionObjectData* objectData, + CollisionGroupData* groupData, + const Option& option, Result& result) { result.contacts.clear(); - assert(object); - assert(group); - assert(object->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - assert(group->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - - object->updateEngineData(); - group->updateEngineData(); + assert(objectData); + assert(groupData); + assert(objectData->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(groupData->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - auto objData = static_cast(object->getEngineData()); - auto groupData = static_cast(group->getEngineData()); + auto castedObjData = static_cast(objectData); + auto castedGrpData = static_cast(groupData); - auto fclObject = objData->getFCLCollisionObject(); - auto broadPhaseAlg = groupData->getFCLCollisionManager(); + auto fclObject = castedObjData->getFCLCollisionObject(); + auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); @@ -208,19 +220,17 @@ bool FCLMeshEngine::detect(CollisionObject* object, CollisionGroup* group, } //============================================================================== -bool FCLMeshEngine::detect(CollisionGroup* group, - const Option& option, Result& result) +bool FCLMeshEngine::detect(CollisionGroupData* groupData, + const Option& option, Result& result) { result.contacts.clear(); - assert(group); - assert(group->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - - group->updateEngineData(); + assert(groupData); + assert(groupData->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - auto data = static_cast(group->getEngineData()); + auto castedData = static_cast(groupData); - auto broadPhaseAlg = data->getFCLCollisionManager(); + auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); broadPhaseAlg->collide(&collData, collisionCallBack); @@ -229,24 +239,22 @@ bool FCLMeshEngine::detect(CollisionGroup* group, } //============================================================================== -bool FCLMeshEngine::detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) +bool FCLMeshEngine::detect(CollisionGroupData* groupData1, + CollisionGroupData* groupData2, + const Option& option, Result& result) { result.contacts.clear(); - assert(group1); - assert(group2); - assert(group1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - assert(group2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - - group1->updateEngineData(); - group2->updateEngineData(); + assert(groupData1); + assert(groupData2); + assert(groupData1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(groupData2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - auto data1 = static_cast(group1->getEngineData()); - auto data2 = static_cast(group2->getEngineData()); + auto castedData1 = static_cast(groupData1); + auto castedData2 = static_cast(groupData2); - auto broadPhaseAlg1 = data1->getFCLCollisionManager(); - auto broadPhaseAlg2 = data2->getFCLCollisionManager(); + auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); + auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); @@ -279,6 +287,9 @@ bool collisionCallBack(fcl::CollisionObject* o1, // return collData->done; // TODO(JS): disabled until other functionalities are implemented + // Clear previous results + fclResult.clear(); + // Perform narrow-phase detection fcl::collide(o1, o2, fclRequest, fclResult); @@ -302,7 +313,10 @@ void postProcess(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o2, Result& result) { - result.contacts.clear(); + auto initNumContacts = fclResult.numContacts(); + + if (0 == initNumContacts) + return; int numCoplanarContacts = 0; int numNoContacts = 0; @@ -359,8 +373,6 @@ void postProcess(const fcl::CollisionResult& fclResult, pair1.triID1 = c.b1; pair1.triID2 = c.b2; - pair1.shape1 = userData1->mShape; - pair1.shape2 = userData2->mShape; pair1.collisionObject1 = collisionObject1; pair1.collisionObject2 = collisionObject2; @@ -369,63 +381,106 @@ void postProcess(const fcl::CollisionResult& fclResult, unfiltered.push_back(pair2); } + + + + const double ZERO = 0.000001; const double ZERO2 = ZERO*ZERO; - std::vector markForDeletion(unfiltered.size(), false); + auto unfilteredSize = unfiltered.size(); + + std::vector markForDeletion(unfilteredSize, false); // mark all the repeated points - for (unsigned int k = 0; k < unfiltered.size(); k++) + for (auto i = 0u; i < unfilteredSize - 1u; ++i) { - for (unsigned int l = k + 1; l < unfiltered.size(); l++) + const auto& contact1 = unfiltered[i]; + + for (auto j = i + 1u; j < unfilteredSize; ++j) { - const Eigen::Vector3d diff = unfiltered[k].point - unfiltered[l].point; + const auto& contact2 = unfiltered[j]; - if (diff.dot(diff) < 3 * ZERO2) + const auto diff = contact1.point - contact2.point; + + if (diff.norm() < 3.0 * ZERO2) { - markForDeletion[k] = true; + markForDeletion[i] = true; break; } } } // remove all the co-linear contact points - for (auto k = 0u; k < unfiltered.size(); ++k) - { - if (markForDeletion[k]) - continue; +// for (auto i = 0u; i < initNumContacts; ++i) +// { +// if (markForDeletion[i]) +// continue; - for (auto l = 0u; l < unfiltered.size(); ++l) - { - if (l == k || markForDeletion[l]) - continue; +// const auto& contact1 = unfiltered[i]; - if (markForDeletion[k]) - break; +// for (auto j = i + 1u; j < initNumContacts; ++j) +// { +// if (markForDeletion[j]) +// continue; - for (auto m = l + 1u; m < unfiltered.size(); ++m) - { - if (k == m) - continue; - - const Eigen::Vector3d va = unfiltered[k].point - unfiltered[l].point; - const Eigen::Vector3d vb = unfiltered[k].point - unfiltered[m].point; - const Eigen::Vector3d v = va.cross(vb); - - if (v.dot(v) < ZERO2 && va.dot(vb) < 0) - { - markForDeletion[k] = true; - break; - } - } +// const auto& contact2 = unfiltered[j]; + +// for (auto k = j + 1u; k < initNumContacts; ++k) +// { +// if (markForDeletion[k]) +// continue; + +// const auto& contact3 = unfiltered[k]; + +// const auto va = contact1.point - contact2.point; +// const auto vb = contact1.point - contact3.point; +// const auto v = va.cross(vb); + +// if (v.norm() < ZERO2) +// { +// markForDeletion[i] = true; +// break; +// } +// } +// } +// } + + for (size_t i = 0; i < initNumContacts; ++i) + { +// if (!markForDeletion[i]) + { + const auto& contact = unfiltered[i]; + result.contacts.push_back(contact); } } - for (size_t k = 0; k < unfiltered.size(); k++) + + + +// for (auto i = 0u; i < numContacts; ++i) +// { +// const auto fclContact = fclResult.getContact(i); +// result.contacts.push_back(convertContact(fclContact, o1, o2)); +// } + + + + std::cout << "=================" << std::endl; + std::cout << "# of contacts:" << result.contacts.size() << std::endl; + std::cout << "=================" << std::endl; + for (auto i = 0u; i < result.contacts.size(); ++i) { - if (!markForDeletion[k]) - result.contacts.push_back(unfiltered[k]); + auto contact = result.contacts[i]; + + std::cout << "Contact (" << i << ")" << std::endl; + std::cout << "Point : " << contact.point.transpose() << std::endl; + std::cout << "Normal: " << contact.normal.transpose() << std::endl; + std::cout << "Depth : " << contact.penetrationDepth << std::endl; + std::cout << std::endl; } + std::cout << "=================" << std::endl; + } //============================================================================== @@ -549,8 +604,6 @@ Contact convertContact(const fcl::Contact& fclContact, = static_cast(o2->getUserData()); assert(userData1); assert(userData2); - contact.shape1 = userData1->mShape; - contact.shape2 = userData2->mShape; contact.collisionObject1 = userData1->mCollisionObject; contact.collisionObject2 = userData2->mCollisionObject; diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.h b/dart/collision/fcl_mesh/FCLMeshEngine.h index dd7c141162d8f..5cbc1a5772ddf 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.h +++ b/dart/collision/fcl_mesh/FCLMeshEngine.h @@ -53,6 +53,12 @@ class FCLMeshEngine : public Engine static FCLMeshEnginePtr create(); + /// Constructor + FCLMeshEngine(); + + /// Constructor + virtual ~FCLMeshEngine(); + /// Return engine type "FCLMesh" static const std::string& getTypeStatic(); @@ -60,34 +66,30 @@ class FCLMeshEngine : public Engine const std::string& getType() const override; // Documentation inherit - std::unique_ptr createCollisionObjectData( + std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; // Documentation inherit - std::unique_ptr createCollisionGroupData( + std::unique_ptr createCollisionGroupData( const CollisionObjectPtrs& collObjects) override; // Documentation inherit - bool detect(CollisionObject* object1, CollisionObject* object2, + bool detect(CollisionObjectData* object1, CollisionObjectData* object2, const Option& option, Result& result) override; // Documentation inherit - bool detect(CollisionObject* object, CollisionGroup* group, + bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) override; // Documentation inherit - bool detect(CollisionGroup* group, + bool detect(CollisionGroupData* group, const Option& option, Result& result) override; // Documentation inherit - bool detect(CollisionGroup* group1, CollisionGroup* group2, + bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; -protected: - - FCLMeshEngine() = default; - }; } // namespace collision diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index b33353c1b1052..97183feed5ccc 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -41,9 +41,10 @@ #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/CollisionDetector.h" #include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/fcl_mesh/FCLMeshEngine.h" #include "dart/collision/CollisionGroup.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" @@ -64,9 +65,9 @@ using namespace dynamics; //============================================================================== ConstraintSolver::ConstraintSolver(double _timeStep) - : mCollisionDetector(new collision::FCLMeshCollisionDetector()), - mCollisionEngine(collision::FCLEngine::create()), - mCollisionGroup(new collision::CollisionGroup(mCollisionEngine)), + : mCollisionDetector( + collision::CollisionDetector::create()), + mCollisionGroup(mCollisionDetector->createCollisionGroup()), mTimeStep(_timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { @@ -76,113 +77,73 @@ ConstraintSolver::ConstraintSolver(double _timeStep) //============================================================================== ConstraintSolver::~ConstraintSolver() { - delete mCollisionDetector; delete mLCPSolver; } //============================================================================== -void ConstraintSolver::addSkeleton(const SkeletonPtr& _skeleton) +void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) { - assert(_skeleton != nullptr + assert(skeleton && "Null pointer skeleton is now allowed to add to ConstraintSover."); - if (containSkeleton(_skeleton) == false) + if (!containSkeleton(skeleton)) { - mSkeletons.push_back(_skeleton); - mCollisionDetector->addSkeleton(_skeleton); - //mCollisionGroup-> + auto group = createShapeFrameCollisionGroup(mCollisionDetector, skeleton); + mCollisionGroup->unionGroup(group); + + mSkeletons.push_back(skeleton); mConstrainedGroups.reserve(mSkeletons.size()); } else { - dtwarn << "Skeleton [" << _skeleton->getName() - << "] is already in ConstraintSolver." << std::endl; + dtwarn << "[ConstraintSolver::addSkeleton] Attempting to add " + << "skeleton '" << skeleton->getName() + << "', which already exists in the ConstraintSolver.\n"; } } //============================================================================== -void ConstraintSolver::addSkeletons(const std::vector& _skeletons) +void ConstraintSolver::addSkeletons(const std::vector& skeletons) { - size_t numAddedSkeletons = 0; - - for (std::vector::const_iterator it = _skeletons.begin(); - it != _skeletons.end(); ++it) - { - assert(*it != nullptr - && "Null pointer skeleton is now allowed to add to ConstraintSover."); - - if (containSkeleton(*it) == false) - { - mSkeletons.push_back(*it); - mCollisionDetector->addSkeleton(*it); - - ++numAddedSkeletons; - } - else - { - dtwarn << "Skeleton [" << (*it)->getName() - << "] is already in ConstraintSolver." << std::endl; - } - } - - mConstrainedGroups.reserve(mSkeletons.size()); + for (const auto skeleton : skeletons) + addSkeleton(skeleton); } //============================================================================== -void ConstraintSolver::removeSkeleton( - const SkeletonPtr& _skeleton) +void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) { - assert(_skeleton != nullptr + assert(skeleton && "Null pointer skeleton is now allowed to add to ConstraintSover."); - if (containSkeleton(_skeleton)) + if (containSkeleton(skeleton)) { - mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), _skeleton), + auto group = createShapeFrameCollisionGroup(mCollisionDetector, skeleton); + mCollisionGroup->subtractGroup(group); + + mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), mSkeletons.end()); - mCollisionDetector->removeSkeleton(_skeleton); mConstrainedGroups.reserve(mSkeletons.size()); } else { - dtwarn << "Skeleton [" << _skeleton->getName() - << "] is not in ConstraintSolver." << std::endl; + dtwarn << "[ConstraintSolver::removeSkeleton] Attempting to remove " + << "skeleton '" << skeleton->getName() + << "', which doesn't exist in the ConstraintSolver.\n"; } } //============================================================================== void ConstraintSolver::removeSkeletons( - const std::vector& _skeletons) + const std::vector& skeletons) { - size_t numRemovedSkeletons = 0; - - for (std::vector::const_iterator it = _skeletons.begin(); - it != _skeletons.end(); ++it) - { - assert(*it != nullptr - && "Null pointer skeleton is now allowed to add to ConstraintSover."); - - if (containSkeleton(*it)) - { - mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), *it), - mSkeletons.end()); - mCollisionDetector->removeSkeleton(*it); - - ++numRemovedSkeletons; - } - else - { - dtwarn << "Skeleton [" << (*it)->getName() - << "] is not in ConstraintSolver." << std::endl; - } - } - - mConstrainedGroups.reserve(mSkeletons.size()); + for (const auto skeleton : skeletons) + removeSkeleton(skeleton); } //============================================================================== void ConstraintSolver::removeAllSkeletons() { - mCollisionDetector->removeAllSkeletons(); + mCollisionGroup->removeAllCollisionObjects(); mSkeletons.clear(); } @@ -242,27 +203,49 @@ double ConstraintSolver::getTimeStep() const //============================================================================== void ConstraintSolver::setCollisionDetector( - collision::CollisionDetector* _collisionDetector) + const collision::CollisionDetectorPtr& detector) { - assert(_collisionDetector && "Invalid collision detector."); - - // Add skeletons in the constraint solver to new collision detector - for (size_t i = 0; i < mSkeletons.size(); ++i) - _collisionDetector->addSkeleton(mSkeletons[i]); - - // Release the old collision detector - delete mCollisionDetector; + assert(detector && "Invalid collision detector."); // Change the collision detector of the constraint solver to new one - mCollisionDetector = _collisionDetector; + mCollisionDetector = detector; + + auto newCollisionGroup = mCollisionDetector->createCollisionGroup( + mCollisionGroup->getCollisionObjects()); + mCollisionGroup = newCollisionGroup; } //============================================================================== -collision::CollisionDetector* ConstraintSolver::getCollisionDetector() const +collision::CollisionDetectorPtr +ConstraintSolver::getCollisionDetector() const { return mCollisionDetector; } +//============================================================================== +collision::CollisionGroup* ConstraintSolver::getCollisionGroup() +{ + return mCollisionGroup.get(); +} + +//============================================================================== +const collision::CollisionGroup* ConstraintSolver::getCollisionGroup() const +{ + return mCollisionGroup.get(); +} + +//============================================================================== +collision::Result ConstraintSolver::getLastCollisionResult() +{ + return mCollisionResult; +} + +//============================================================================== +const collision::Result& ConstraintSolver::getLastCollisionResult() const +{ + return mCollisionResult; +} + //============================================================================== void ConstraintSolver::solve() { @@ -355,8 +338,11 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- // Update automatic constraints: contact constraints //---------------------------------------------------------------------------- - mCollisionDetector->clearAllContacts(); - mCollisionDetector->detectCollision(true, true); + mCollisionResult.contacts.clear(); + + collision::Option option; + option.enableContact = true; + mCollisionGroup->detect(option, mCollisionResult); // Destroy previous contact constraints mContactConstraints.clear(); @@ -365,9 +351,9 @@ void ConstraintSolver::updateConstraints() mSoftContactConstraints.clear(); // Create new contact constraints - for (size_t i = 0; i < mCollisionDetector->getNumContacts(); ++i) + for (auto i = 0u; i < mCollisionResult.contacts.size(); ++i) { - collision::Contact& ct = mCollisionDetector->getContact(i); + collision::Contact& ct = mCollisionResult.contacts[i]; if (isSoftContact(ct)) { @@ -544,11 +530,18 @@ void ConstraintSolver::solveConstrainedGroups() //============================================================================== bool ConstraintSolver::isSoftContact(const collision::Contact& _contact) const { - if (dynamic_cast(_contact.bodyNode1.lock().get()) - || dynamic_cast(_contact.bodyNode2.lock().get())) - return true; + auto shapeFrameCollObj1 = static_cast( + _contact.collisionObject1); + auto shapeFrameCollObj2 = static_cast( + _contact.collisionObject2); - return false; + auto bodyNode1IsSoft = + dynamic_cast(shapeFrameCollObj1) != nullptr; + + auto bodyNode2IsSoft = + dynamic_cast(shapeFrameCollObj2) != nullptr; + + return bodyNode1IsSoft || bodyNode2IsSoft; } } // namespace constraint diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index 88e4f07eb4a66..dce01dd5677b6 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -50,6 +50,7 @@ namespace dart { namespace dynamics { class Skeleton; +class ShapeFrameCollisionObject; } // namespace dynamics namespace constraint { @@ -73,16 +74,16 @@ class ConstraintSolver virtual ~ConstraintSolver(); /// Add single skeleton - void addSkeleton(const dynamics::SkeletonPtr& _skeleton); + void addSkeleton(const dynamics::SkeletonPtr& skeleton); /// Add mutiple skeletons - void addSkeletons(const std::vector& _skeletons); + void addSkeletons(const std::vector& skeletons); /// Remove single skeleton - void removeSkeleton(const dynamics::SkeletonPtr& _skeleton); + void removeSkeleton(const dynamics::SkeletonPtr& skeleton); /// Remove multiple skeletons - void removeSkeletons(const std::vector& _skeletons); + void removeSkeletons(const std::vector& skeletons); /// Remove all skeletons in this constraint solver void removeAllSkeletons(); @@ -103,10 +104,24 @@ class ConstraintSolver double getTimeStep() const; /// Set collision detector - void setCollisionDetector(collision::CollisionDetector* _collisionDetector); + void setCollisionDetector(const collision::CollisionDetectorPtr& detector); /// Get collision detector - collision::CollisionDetector* getCollisionDetector() const; + collision::CollisionDetectorPtr getCollisionDetector() const; + + /// Return collision group of collision objects that are added to this + /// ConstraintSolver + collision::CollisionGroup* getCollisionGroup(); + + /// Return (const) collision group of collision objects that are added to this + /// ConstraintSolver + const collision::CollisionGroup* getCollisionGroup() const; + + /// Return the last collision checking result + collision::Result getLastCollisionResult(); + + /// Return the last collision checking result + const collision::Result& getLastCollisionResult() const; /// Solve constraint impulses and apply them to the skeletons void solve(); @@ -136,14 +151,16 @@ class ConstraintSolver /// Return true if at least one of colliding body is soft body bool isSoftContact(const collision::Contact& _contact) const; - /// Collision detector - collision::CollisionDetector* mCollisionDetector; + using CollisionDetector = collision::CollisionDetector; /// Collision detection engine - std::shared_ptr mCollisionEngine; + std::shared_ptr mCollisionDetector; /// Collision group - std::unique_ptr mCollisionGroup; + std::shared_ptr mCollisionGroup; + + /// Last collision checking result + collision::Result mCollisionResult; /// Time step double mTimeStep; diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 45fb9c55691f2..b20516826f8ee 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -42,6 +42,7 @@ #include "dart/math/Helpers.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/CollisionDetector.h" #include "dart/lcpsolver/lcp.h" #define DART_ERROR_ALLOWANCE 0.0 @@ -69,6 +70,10 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, double _timeStep) : ConstraintBase(), mTimeStep(_timeStep), + mBodyNode1(static_cast( + _contact.collisionObject1)->getBodyNode()), + mBodyNode2(static_cast( + _contact.collisionObject2)->getBodyNode()), mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), mIsFrictionOn(true), mAppliedImpulseIndex(-1), @@ -78,10 +83,6 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, // TODO(JS): Assumed single contact mContacts.push_back(&_contact); - // TODO(JS): - mBodyNode1 = _contact.bodyNode1.lock(); - mBodyNode2 = _contact.bodyNode2.lock(); - //---------------------------------------------- // Bounce //---------------------------------------------- diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index dfa9ae941f94d..3dc0c3d03ded3 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -45,7 +45,7 @@ #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/Shape.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/dynamics/CollisionDetector.h" #include "dart/lcpsolver/lcp.h" #define DART_ERROR_ALLOWANCE 0.0 @@ -73,8 +73,10 @@ SoftContactConstraint::SoftContactConstraint( collision::Contact& _contact, double _timeStep) : ConstraintBase(), mTimeStep(_timeStep), - mBodyNode1(_contact.bodyNode1.lock()), - mBodyNode2(_contact.bodyNode2.lock()), + mBodyNode1(static_cast( + _contact.collisionObject1)->getBodyNode()), + mBodyNode2(static_cast( + _contact.collisionObject2)->getBodyNode()), mSoftBodyNode1(dynamic_cast(mBodyNode1)), mSoftBodyNode2(dynamic_cast(mBodyNode2)), mPointMass1(nullptr), @@ -104,7 +106,8 @@ SoftContactConstraint::SoftContactConstraint( // Select colling point mass based on trimesh ID if (mSoftBodyNode1) { - if (_contact.shape1->getShapeType() == dynamics::Shape::SOFT_MESH) + if (_contact.collisionObject1->getShape()->getShapeType() + == dynamics::Shape::SOFT_MESH) { mPointMass1 = selectCollidingPointMass(mSoftBodyNode1, _contact.point, _contact.triID1); @@ -112,7 +115,8 @@ SoftContactConstraint::SoftContactConstraint( } if (mSoftBodyNode2) { - if (_contact.shape2->getShapeType() == dynamics::Shape::SOFT_MESH) + if (_contact.collisionObject2->getShape()->getShapeType() + == dynamics::Shape::SOFT_MESH) { mPointMass2 = selectCollidingPointMass(mSoftBodyNode2, _contact.point, _contact.triID2); diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index bfafad8be6ab1..17151336732ee 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -36,15 +36,17 @@ #include "dart/dynamics/CollisionDetector.h" +#include "dart/dynamics/BodyNode.h" + namespace dart { namespace dynamics { //============================================================================== -ShapeNodeCollisionObject::ShapeNodeCollisionObject( - const collision::EnginePtr& engine, +ShapeFrameCollisionObject::ShapeFrameCollisionObject( + const collision::CollisionDetectorPtr& collisionDetector, const dynamics::ShapePtr& shape, const dynamics::BodyNodePtr& bodyNode) - : CollisionObject(engine, shape), + : collision::CollisionObject(collisionDetector, shape), mBodyNode(bodyNode) { auto found = false; @@ -61,39 +63,38 @@ ShapeNodeCollisionObject::ShapeNodeCollisionObject( if (!found) { - dtwarn << "[ShapeNodeCollisionObject::constructor] Attempting to create " - << "ShapeNodeCollisionObject with invalid pair of Shape and " + dtwarn << "[ShapeFrameCollisionObject::constructor] Attempting to create " + << "ShapeFrameCollisionObject with invalid pair of Shape and " << "BodyNode.\n"; assert(false); } } //============================================================================== -const Eigen::Isometry3d ShapeNodeCollisionObject::getTransform() const +const Eigen::Isometry3d ShapeFrameCollisionObject::getTransform() const { return mBodyNode->getWorldTransform() * mShape->getLocalTransform(); } //============================================================================== -BodyNodePtr ShapeNodeCollisionObject::getBodyNode() const +BodyNodePtr ShapeFrameCollisionObject::getBodyNode() const { return mBodyNode; } //============================================================================== -std::shared_ptr -CollisionDetector::createCollisionObject( - const collision::EnginePtr engine, +ShapeFrameCollisionObjectPtr createShapeFrameCollisionObject( + const collision::CollisionDetectorPtr& collisionDetector, const ShapePtr& shape, const BodyNodePtr& bodyNode) { - return std::make_shared(engine, shape, bodyNode); + return collisionDetector->createCollisionObject( + shape, bodyNode); } //============================================================================== -std::vector -CollisionDetector::createCollisionObjects( - const collision::EnginePtr& engine, +std::vector createShapeFrameCollisionObjects( + const collision::CollisionDetectorPtr& collisionDetector, const SkeletonPtr& skel) { std::vector objects; @@ -107,12 +108,56 @@ CollisionDetector::createCollisionObjects( for (auto j = 0u; j < numColShapes; ++j) { auto shape = bodyNode->getCollisionShape(j); - objects.push_back(createCollisionObject(engine, shape, bodyNode)); + auto collObj = collisionDetector->createCollisionObject< + ShapeFrameCollisionObject>(shape, bodyNode); + + objects.push_back( + std::static_pointer_cast(collObj)); } } return objects; } +//============================================================================== +collision::CollisionGroupPtr createShapeFrameCollisionGroup( + const collision::CollisionDetectorPtr& collisionDetector, + const SkeletonPtr& skel) +{ + auto collObjs = createShapeFrameCollisionObjects(collisionDetector, skel); + auto group = collisionDetector->createCollisionGroup(collObjs); + + return group; +} + +////============================================================================== +//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, +// const ShapePtr& shape1, const BodyNodePtr& body1, +// const ShapePtr& shape2, const BodyNodePtr& body2, +// const collision::Option& option, +// collision::Result& result) +//{ +// auto obj1 = collisionDetector->createCollisionObject(shape1, body1); +// auto obj2 = collisionDetector->createCollisionObject(shape2, body2); + +// return obj1->detect(obj2.get(), option, result); +//} + +////============================================================================== +//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, +// const SkeletonPtr& skel1, +// const SkeletonPtr& skel2, +// const collision::Option& option, +// collision::Result& result) +//{ +// auto objects1 = createShapeFrameCollisionObjects(collisionDetector, skel1); +// auto objects2 = createShapeFrameCollisionObjects(collisionDetector, skel2); + +// auto group1 = collisionDetector->createCollisionGroup(objects1); +// auto group2 = collisionDetector->createCollisionGroup(objects2); + +// return group1->detect(group2.get(), option, result); +//} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index 22b2ba383117a..d1a64796d3ff5 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -40,29 +40,28 @@ #include #include "dart/collision/CollisionDetector.h" -#include "dart/dynamics/SmartPointer.h" #include "dart/collision/Engine.h" #include "dart/collision/fcl/FCLEngine.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionGroup.h" -#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/SmartPointer.h" namespace dart { namespace dynamics { -class ShapeNodeCollisionObject : public collision::CollisionObject +class ShapeFrameCollisionObject : public collision::CollisionObject { public: - ShapeNodeCollisionObject(const collision::EnginePtr& engine, - const dynamics::ShapePtr& shape, - const dynamics::BodyNodePtr& bodyNode); + ShapeFrameCollisionObject( + const collision::CollisionDetectorPtr& collisionDetector, + const ShapePtr& shape, + const BodyNodePtr& bodyNode); // TODO(JS): this should be replaced by ShapeNode // Documentation inherited const Eigen::Isometry3d getTransform() const override; - /// Return BodyNode pointer associated with this ShapeNodeCollisionObject + /// Return BodyNode pointer associated with this ShapeFrameCollisionObject dynamics::BodyNodePtr getBodyNode() const; protected: @@ -72,70 +71,49 @@ class ShapeNodeCollisionObject : public collision::CollisionObject }; -class CollisionDetector -{ -public: - - static std::shared_ptr createCollisionObject( - const collision::EnginePtr engine, - const dynamics::ShapePtr& shape, - const BodyNodePtr& bodyNode); - - static std::vector - createCollisionObjects( - const collision::EnginePtr& engine, - const dynamics::SkeletonPtr& skel); - - template - static bool detect(const dynamics::ShapePtr& shape1, - const BodyNodePtr& body1, - const dynamics::ShapePtr& shape2, - const BodyNodePtr& body2, - const collision::Option& option, - collision::Result& result); - - template - static bool detect(const dynamics::SkeletonPtr& skel1, - const dynamics::SkeletonPtr& skel2, - const collision::Option& option, - collision::Result& result); - -}; - -//============================================================================== -template -bool CollisionDetector::detect( - const ShapePtr& shape1, const BodyNodePtr& body1, - const ShapePtr& shape2, const BodyNodePtr& body2, - const collision::Option& option, collision::Result& result) -{ - auto engine = ColDecEngine::create(); - - auto obj1 = ShapeNodeCollisionObject(engine, shape1, body1); - auto obj2 = ShapeNodeCollisionObject(engine, shape2, body2); - - return obj1.detect(&obj2, option, result); -} - -//============================================================================== -template -bool CollisionDetector::detect( - const dynamics::SkeletonPtr& skel1, - const dynamics::SkeletonPtr& skel2, - const collision::Option& option, collision::Result& result) -{ - auto engine = ColDecEngine::create(); - - auto objects1 = createCollisionObjects(engine, skel1); - auto objects2 = createCollisionObjects(engine, skel2); - - auto group1 = collision::CollisionGroup(engine, objects1); - auto group2 = collision::CollisionGroup(engine, objects2); - - return group1.detect(&group2, option, result); -} +using ShapeFrameCollisionObjectPtr = std::shared_ptr; + +/// Create a ShapeFrameCollisionObject given ShapeNode +ShapeFrameCollisionObjectPtr createShapeFrameCollisionObject( + const collision::CollisionDetectorPtr& collisionDetector, + const ShapePtr& shape, + const BodyNodePtr& bodyNode); + +/// Create a ShapeFrameCollisionObjects given Skeleton +std::vector createShapeFrameCollisionObjects( + const collision::CollisionDetectorPtr& collisionDetector, + const dynamics::SkeletonPtr& skel); + +/// Create a CollisionGroup given Skeleton +collision::CollisionGroupPtr createShapeFrameCollisionGroup( + const collision::CollisionDetectorPtr& collisionDetector, + const dynamics::SkeletonPtr& skel); + +///// Checks the collisions between two BodyNodes for one time. +//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, +// const dynamics::ShapePtr& shape1, +// const BodyNodePtr& body1, +// const dynamics::ShapePtr& shape2, +// const BodyNodePtr& body2, +// const collision::Option& option, +// collision::Result& result); + +///// Checks the collisions between two Skeletons for one time. +///// +///// This function creates two collision groups internally as local variables +///// of the skeletons for single time collision check. If you want to check +///// more than once, then create two collision groups explicitly using +///// createShapeNodeCollisionGroup() and check the collisions with the groups +///// for better performance. +//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, +// const dynamics::SkeletonPtr& skel1, +// const dynamics::SkeletonPtr& skel2, +// const collision::Option& option, +// collision::Result& result); } // namespace dynamics } // namespace dart +#include "dart/dynamics/detail/CollisionDetector.h" + #endif // DART_DYNAMICS_COLLISIONDETECTOR_H_ diff --git a/dart/dynamics/CollisionGroupGenerator.h b/dart/dynamics/CollisionGroupGenerator.h deleted file mode 100644 index f2dc98613957a..0000000000000 --- a/dart/dynamics/CollisionGroupGenerator.h +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_DYNAMICS_COLLISIONGROUPGENERATOR_H_ -#define DART_DYNAMICS_COLLISIONGROUPGENERATOR_H_ - -#include - -#include "dart/collision/CollisionDetector.h" -#include "dart/dynamics/SmartPointer.h" - -namespace dart { -namespace dynamics { - -namespace CollisionNodeGenerator -{ -// collision::CollisionNode2* create(const BodyNodePtr& bodyNode); -} - -namespace CollisionGroupGenerator -{ -// collision::CollisionGroupPtr generate( -// const collision::CollisionDetectorPtr& cd, -// const SkeletonPtr& skeleton); - -// CollisionGroupPtr -// createGroup(const ShapeNodePair& shape) { return nullptr; } - -// CollisionGroupPtr -// createGroup(const dynamics::BodyNodePtr& bodyNode) { return nullptr; } - -// CollisionGroupPtr -// createGroup(const dynamics::SkeletonPtr& Skeleton) { return nullptr; } -} - -} // namespace dynamics -} // namespace dart - -#endif // DART_DYNAMICS_NODE_H_ diff --git a/dart/dynamics/CollisionGroup.h b/dart/dynamics/detail/CollisionDetector.h similarity index 87% rename from dart/dynamics/CollisionGroup.h rename to dart/dynamics/detail/CollisionDetector.h index 8646e1042d93e..313a626833e4b 100644 --- a/dart/dynamics/CollisionGroup.h +++ b/dart/dynamics/detail/CollisionDetector.h @@ -34,26 +34,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_COLLISIONGROUP_H_ -#define DART_DYNAMICS_COLLISIONGROUP_H_ - -#include +#ifndef DART_DYNAMICS_DETAIL_COLLISIONDETECTOR_H_ +#define DART_DYNAMICS_DETAIL_COLLISIONDETECTOR_H_ #include "dart/collision/CollisionGroup.h" namespace dart { namespace dynamics { -class CollisionGroup : public collision::CollisionGroup -{ -public: - - CollisionGroup(const collision::EnginePtr& engine); - - void addCollisionObjects(const SkeletonPtr& skeleton); - -}; - } // namespace dynamics } // namespace dart diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 8b705d32e6587..cfc26a0502789 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -46,8 +46,8 @@ #include "dart/collision/bullet/BulletCollisionDetector.h" #endif #include "dart/collision/dart/DARTCollisionDetector.h" -#include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/fcl_mesh/FCLMeshEngine.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/SoftBodyNode.h" @@ -256,27 +256,30 @@ simulation::WorldPtr SkelParser::readWorld( if (strCD == "fcl_mesh") { newWorld->getConstraintSolver()->setCollisionDetector( - new collision::FCLMeshCollisionDetector()); + collision::CollisionDetector::create()); } else if (strCD == "fcl") { newWorld->getConstraintSolver()->setCollisionDetector( - new collision::FCLCollisionDetector()); + collision::CollisionDetector::create()); } - else if (strCD == "dart") - { - newWorld->getConstraintSolver()->setCollisionDetector( - new collision::DARTCollisionDetector()); - } -#ifdef HAVE_BULLET_COLLISION - else if (strCD == "bullet") - { - newWorld->getConstraintSolver()->setCollisionDetector( - new collision::BulletCollisionDetector()); - } -#endif +// else if (strCD == "dart") +// { +// newWorld->getConstraintSolver()->setCollisionEngine( +// new collision::DARTEngine()); +// } +//#ifdef HAVE_BULLET_COLLISION +// else if (strCD == "bullet") +// { +// newWorld->getConstraintSolver()->setCollisionEngine( +// new collision::BulletEngine()); +// } +//#endif + // TODO(JS) else { + newWorld->getConstraintSolver()->setCollisionDetector( + collision::CollisionDetector::create()); dtwarn << "Unknown collision detector[" << strCD << "]. " << "Default collision detector[fcl] will be loaded." << std::endl; @@ -285,7 +288,7 @@ simulation::WorldPtr SkelParser::readWorld( else { newWorld->getConstraintSolver()->setCollisionDetector( - new collision::FCLMeshCollisionDetector()); + collision::CollisionDetector::create()); } } @@ -626,7 +629,7 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( } //-------------------------------------------------------------------------- - // visualization_shape + // collision_shape ElementEnumerator collShapes(_bodyNodeElement, "collision_shape"); while (collShapes.next()) { @@ -1477,12 +1480,10 @@ static void readJointDynamicsAndLimit(tinyxml2::XMLElement* _jointElement, //============================================================================== SkelParser::JointPropPtr SkelParser::readWeldJoint( - tinyxml2::XMLElement* _jointElement, + tinyxml2::XMLElement* /*_jointElement*/, SkelJoint& /*_joint*/, const std::string&) { - assert(_jointElement != nullptr); - return Eigen::make_aligned_shared(); } diff --git a/dart/utils/sdf/SoftSdfParser.cpp b/dart/utils/sdf/SoftSdfParser.cpp index 7851278d6c874..df3bc2c2703c8 100644 --- a/dart/utils/sdf/SoftSdfParser.cpp +++ b/dart/utils/sdf/SoftSdfParser.cpp @@ -42,8 +42,6 @@ #include "dart/common/Console.h" #include "dart/collision/dart/DARTCollisionDetector.h" -#include "dart/collision/fcl/FCLCollisionDetector.h" -// #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/CylinderShape.h" @@ -59,7 +57,6 @@ #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/dynamics/SoftMeshShape.h" #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Skeleton.h" diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index b767f3daad204..6caa7c80fa30e 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -473,8 +473,9 @@ int main(int argc, char* argv[]) world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); #ifdef HAVE_BULLET_COLLISION - world->getConstraintSolver()->setCollisionDetector( - new dart::collision::BulletCollisionDetector()); + // world->getConstraintSolver()->setCollisionEngine( + // dart::collision::BulletEngine::create()); + // TODO(JS) #endif world->addSkeleton(floor); diff --git a/tutorials/tutorialBiped.cpp b/tutorials/tutorialBiped.cpp index 582a1f2c47263..8baa801584c1b 100644 --- a/tutorials/tutorialBiped.cpp +++ b/tutorials/tutorialBiped.cpp @@ -311,8 +311,9 @@ int main(int argc, char* argv[]) world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); #ifdef HAVE_BULLET_COLLISION - world->getConstraintSolver()->setCollisionDetector( - new dart::collision::BulletCollisionDetector()); +// world->getConstraintSolver()->setCollisionEngine( +// dart::collision::BulletEngine::create()); + // TODO(JS) #endif world->addSkeleton(floor); diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index f2a63f7fcb539..a90f0ff3deb87 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -210,32 +210,25 @@ class MyWindow : public dart::gui::SimWindow // Add the object to the world object->setName(object->getName()+std::to_string(mSkelCount++)); - mWorld->addSkeleton(object); - - // Compute collisions - dart::collision::CollisionDetector* detector = - mWorld->getConstraintSolver()->getCollisionDetector(); - detector->detectCollision(true, true); // Look through the collisions to see if the new object would start in // collision with something - bool collision = false; - size_t collisionCount = detector->getNumContacts(); - for(size_t i = 0; i < collisionCount; ++i) + auto collisionEngine = mWorld->getConstraintSolver()->getCollisionEngine(); + auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); + auto newGroup = createShapeNodeCollisionGroup(collisionEngine, object); + + dart::collision::Option option; + dart::collision::Result result; + bool collision = collisionGroup->detect(newGroup.get(), option, result); + + // If the new object is not in collision + if(!collision) { - const dart::collision::Contact& contact = detector->getContact(i); - if(contact.bodyNode1.lock()->getSkeleton() == object - || contact.bodyNode2.lock()->getSkeleton() == object) - { - collision = true; - break; - } + mWorld->addSkeleton(object); } - - // Refuse to add the object if it is in collision - if(collision) + else { - mWorld->removeSkeleton(object); + // or refuse to add the object if it is in collision std::cout << "The new object spawned in a collision. " << "It will not be added to the world." << std::endl; return false; diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 06e0cd5f374f1..c7a9a7d5f0079 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -262,36 +262,19 @@ class MyWindow : public dart::gui::SimWindow newDomino->setPositions(x); - mWorld->addSkeleton(newDomino); - - // Compute collisions - dart::collision::CollisionDetector* detector = - mWorld->getConstraintSolver()->getCollisionDetector(); - detector->detectCollision(true, true); - // Look through the collisions to see if any dominoes are penetrating // something - bool dominoCollision = false; - size_t collisionCount = detector->getNumContacts(); - for(size_t i = 0; i < collisionCount; ++i) - { - // If neither of the colliding BodyNodes belongs to the floor, then we - // know the new domino is in contact with something it shouldn't be - const dart::collision::Contact& contact = detector->getContact(i); - if(contact.bodyNode1.lock()->getSkeleton() != mFloor - && contact.bodyNode2.lock()->getSkeleton() != mFloor) - { - dominoCollision = true; - break; - } - } + auto collisionEngine = mWorld->getConstraintSolver()->getCollisionEngine(); + auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); + auto newGroup = createShapeNodeCollisionGroup(collisionEngine, newDomino); - if(dominoCollision) - { - // Remove the new domino, because it is penetrating an existing one - mWorld->removeSkeleton(newDomino); - } - else + dart::collision::Option option; + dart::collision::Result result; + bool dominoCollision + = collisionGroup->detect(newGroup.get(), option, result); + + // If the new domino is not penetrating an existing one + if(!dominoCollision) { // Record the latest domino addition mAngles.push_back(angle); diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 0db5461a89f96..323a74a720741 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -55,6 +55,7 @@ using namespace dart; using namespace common; using namespace math; +using namespace collision; using namespace dynamics; using namespace simulation; using namespace utils; @@ -511,22 +512,20 @@ TEST_F(COLLISION, FCL_BOX_BOX) template void testFreeCollisionObjects() { - auto engine = EngineType::create(); + auto cd = CollisionDetector::create(); ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - collision::FreeCollisionObjectPtr obj1( - new collision::FreeCollisionObject(engine, shape1)); - collision::FreeCollisionObjectPtr obj2( - new collision::FreeCollisionObject(engine, shape2)); - collision::FreeCollisionObjectPtr obj3( - new collision::FreeCollisionObject(engine, shape3)); + // Create collision objects in different ways + auto obj1 = cd->template createCollisionObject(shape1); + auto obj2 = FreeCollisionObject::create(cd, shape2); + auto obj3 = std::shared_ptr(new FreeCollisionObject(cd, shape3)); - collision::CollisionGroupPtr group1(new collision::CollisionGroup(engine)); + collision::CollisionGroupPtr group1(new collision::CollisionGroup(cd)); group1->addCollisionObject(obj1); - collision::CollisionGroupPtr group2(new collision::CollisionGroup(engine)); + collision::CollisionGroupPtr group2(new collision::CollisionGroup(cd)); group2->addCollisionObject(obj2); group2->addCollisionObject(obj3); @@ -576,7 +575,7 @@ TEST_F(COLLISION, FreeCollisionObjects) template void testBodyNodes() { - auto engine = EngineType::create(); + auto cd = CollisionDetector::create(); Eigen::Vector3d size(1.0, 1.0, 1.0); Eigen::Vector3d pos1(0.0, 0.0, 0.0); @@ -594,11 +593,13 @@ void testBodyNodes() collision::Option option; collision::Result result; - auto hit = dynamics::CollisionDetector::detect(boxShape1, boxBody1, - boxShape2, boxBody2, - option, result); + auto obj1 = cd->template createCollisionObject( + boxShape1, boxBody1); + auto obj2 = cd->template createCollisionObject( + boxShape2, boxBody2); - EXPECT_TRUE(hit); + EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); + EXPECT_TRUE(cd->detect(obj1.get(), obj2.get(), option, result)); } //============================================================================== @@ -612,22 +613,21 @@ TEST_F(COLLISION, BodyNodeNodes) template void testSkeletons() { - auto engine = EngineType::create(); +// auto engine = EngineType::create(); - Eigen::Vector3d size(1.0, 1.0, 1.0); - Eigen::Vector3d pos1(0.0, 0.0, 0.0); - Eigen::Vector3d pos2(0.5, 0.0, 0.0); +// Eigen::Vector3d size(1.0, 1.0, 1.0); +// Eigen::Vector3d pos1(0.0, 0.0, 0.0); +// Eigen::Vector3d pos2(0.5, 0.0, 0.0); - auto boxSkel1 = createBox(size, pos1); - auto boxSkel2 = createBox(size, pos2); +// auto boxSkel1 = createBox(size, pos1); +// auto boxSkel2 = createBox(size, pos2); - collision::Option option; - collision::Result result; +// collision::Option option; +// collision::Result result; - auto hit = dynamics::CollisionDetector::detect( - boxSkel1, boxSkel2, option, result); +// auto hit = dynamics::detect(engine, boxSkel1, boxSkel2, option, result); - EXPECT_TRUE(hit); +// EXPECT_TRUE(hit); } //============================================================================== From 889ccf8c7f4573d7f9a5ba172eb972d02b93bc61 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 26 Feb 2016 10:04:29 -0500 Subject: [PATCH 09/67] Implement DARTEngine --- dart/collision/CollisionGroup.cpp | 7 +- dart/collision/CollisionGroupData.cpp | 24 +- dart/collision/CollisionGroupData.h | 11 +- dart/collision/CollisionObjectData.cpp | 28 +- dart/collision/CollisionObjectData.h | 10 +- dart/collision/Engine.h | 1 + dart/collision/SmartPointer.h | 1 + dart/collision/dart/DARTCollisionDetector.cpp | 162 ---------- .../collision/dart/DARTCollisionGroupData.cpp | 97 ++++++ dart/collision/dart/DARTCollisionGroupData.h | 84 +++++ .../dart/DARTCollisionObjectData.cpp | 88 ++++++ ...onDetector.h => DARTCollisionObjectData.h} | 40 +-- dart/collision/dart/DARTEngine.cpp | 291 ++++++++++++++++++ dart/collision/dart/DARTEngine.h | 97 ++++++ dart/constraint/ConstraintSolver.cpp | 29 +- dart/gui/SimWindow.cpp | 11 +- dart/simulation/World.cpp | 34 +- dart/utils/SkelParser.cpp | 12 +- dart/utils/sdf/SoftSdfParser.cpp | 1 - tutorials/tutorialCollisions-Finished.cpp | 4 +- tutorials/tutorialDominoes-Finished.cpp | 4 +- unittests/testConstraint.cpp | 17 +- 22 files changed, 815 insertions(+), 238 deletions(-) delete mode 100644 dart/collision/dart/DARTCollisionDetector.cpp create mode 100644 dart/collision/dart/DARTCollisionGroupData.cpp create mode 100644 dart/collision/dart/DARTCollisionGroupData.h create mode 100644 dart/collision/dart/DARTCollisionObjectData.cpp rename dart/collision/dart/{DARTCollisionDetector.h => DARTCollisionObjectData.h} (69%) create mode 100644 dart/collision/dart/DARTEngine.cpp create mode 100644 dart/collision/dart/DARTEngine.h diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 6416a18ac02f7..00425450a6bd3 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -52,7 +52,7 @@ CollisionGroup::CollisionGroup( : mCollisionDetector(collisionDetector), mCollisionObjects(collObjects), mEngineData(mCollisionDetector->getEngine()->createCollisionGroupData( - mCollisionObjects).release()) + this, mCollisionObjects).release()) { assert(mCollisionDetector); } @@ -61,7 +61,7 @@ CollisionGroup::CollisionGroup( CollisionGroup::CollisionGroup(const CollisionGroup& other) : mCollisionDetector(other.mCollisionDetector), mCollisionObjects(other.mCollisionObjects), - mEngineData(other.mEngineData->clone(mCollisionObjects)) + mEngineData(other.mEngineData->clone(this, mCollisionObjects)) { // Do nothing } @@ -84,7 +84,8 @@ void CollisionGroup::copy(const CollisionGroup& other) { mCollisionDetector = other.mCollisionDetector; mCollisionObjects = other.mCollisionObjects; - mEngineData.reset(other.mEngineData->clone(mCollisionObjects).release()); + mEngineData.reset( + other.mEngineData->clone(this, mCollisionObjects).release()); } //============================================================================== diff --git a/dart/collision/CollisionGroupData.cpp b/dart/collision/CollisionGroupData.cpp index ddca16e8b7ff9..50dcc6bd36b27 100644 --- a/dart/collision/CollisionGroupData.cpp +++ b/dart/collision/CollisionGroupData.cpp @@ -42,10 +42,18 @@ namespace dart { namespace collision { //============================================================================== -CollisionGroupData::CollisionGroupData(Engine* engine) - : mEngine(engine) +CollisionGroupData::CollisionGroupData(Engine* engine, CollisionGroup* parent) + : mEngine(engine), + mParent(parent) { assert(mEngine); + assert(mParent); +} + +//============================================================================== +Engine* CollisionGroupData::getEngine() +{ + return mEngine; } //============================================================================== @@ -54,5 +62,17 @@ const Engine* CollisionGroupData::getEngine() const return mEngine; } +//============================================================================== +CollisionGroup* CollisionGroupData::getCollisionGroup() +{ + return mParent; +} + +//============================================================================== +const CollisionGroup* CollisionGroupData::getCollisionGroup() const +{ + return mParent; +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroupData.h b/dart/collision/CollisionGroupData.h index 0778ecf4e271e..f4682286dfb8a 100644 --- a/dart/collision/CollisionGroupData.h +++ b/dart/collision/CollisionGroupData.h @@ -51,7 +51,7 @@ class CollisionGroupData using CollisionObjectPtrs = std::vector; - CollisionGroupData(Engine* engine); + CollisionGroupData(Engine* engine, CollisionGroup* parent); virtual void init() = 0; @@ -71,14 +71,23 @@ class CollisionGroupData virtual void update() = 0; virtual std::unique_ptr clone( + CollisionGroup* newCollisionGroup, const CollisionObjectPtrs& collObjects) const = 0; + Engine* getEngine(); + const Engine* getEngine() const; + CollisionGroup* getCollisionGroup(); + + const CollisionGroup* getCollisionGroup() const; + protected: Engine* mEngine; + CollisionGroup* mParent; + }; } // namespace collision diff --git a/dart/collision/CollisionObjectData.cpp b/dart/collision/CollisionObjectData.cpp index 041834dffde88..0940b2315ed9c 100644 --- a/dart/collision/CollisionObjectData.cpp +++ b/dart/collision/CollisionObjectData.cpp @@ -40,10 +40,20 @@ namespace dart { namespace collision { //============================================================================== -CollisionObjectData::CollisionObjectData(Engine* engine) - : mEngine(engine) +CollisionObjectData::CollisionObjectData( + Engine* engine, + CollisionObject* parent) + : mEngine(engine), + mParent(parent) { - // Do nothing + assert(mEngine); + assert(mParent); +} + +//============================================================================== +Engine* CollisionObjectData::getEngine() +{ + return mEngine; } //============================================================================== @@ -52,5 +62,17 @@ const Engine* CollisionObjectData::getEngine() const return mEngine; } +//============================================================================== +CollisionObject* CollisionObjectData::getCollisionObject() +{ + return mParent; +} + +//============================================================================== +const CollisionObject* CollisionObjectData::getCollisionObject() const +{ + return mParent; +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObjectData.h b/dart/collision/CollisionObjectData.h index e33d1700fdf57..5211fc200e174 100644 --- a/dart/collision/CollisionObjectData.h +++ b/dart/collision/CollisionObjectData.h @@ -49,7 +49,7 @@ class CollisionObjectData { public: - CollisionObjectData(Engine* engine); + CollisionObjectData(Engine* engine, CollisionObject* parent); virtual void updateTransform(const Eigen::Isometry3d& tf) = 0; @@ -60,12 +60,20 @@ class CollisionObjectData virtual void update() = 0; // TODO(JS): reorganize updateTansform/updateShape/update + Engine* getEngine(); + const Engine* getEngine() const; + CollisionObject* getCollisionObject(); + + const CollisionObject* getCollisionObject() const; + protected: Engine* mEngine; + CollisionObject* mParent; + }; } // namespace collision diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h index 370d34ed92343..283e844163418 100644 --- a/dart/collision/Engine.h +++ b/dart/collision/Engine.h @@ -63,6 +63,7 @@ class Engine /// Create collision detection engine specific data for CollisionGroup virtual std::unique_ptr createCollisionGroupData( + CollisionGroup* parent, const CollisionObjectPtrs& collObjects) = 0; /// Perform collision detection for object1-object2. diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h index 08cb0ef611a56..df6ec7fb6e72c 100644 --- a/dart/collision/SmartPointer.h +++ b/dart/collision/SmartPointer.h @@ -48,6 +48,7 @@ DART_COMMON_MAKE_SHARED_WEAK(CollisionDetector) DART_COMMON_MAKE_SHARED_WEAK(Engine) DART_COMMON_MAKE_SHARED_WEAK(FCLEngine) DART_COMMON_MAKE_SHARED_WEAK(FCLMeshEngine) +DART_COMMON_MAKE_SHARED_WEAK(DARTEngine) #ifdef HAVE_BULLET_COLLISION DART_COMMON_MAKE_SHARED_WEAK(BulletEngine) #endif diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp deleted file mode 100644 index f8a4d673072eb..0000000000000 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/dart/DARTCollisionDetector.h" - -#include - -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/collision/dart/DARTCollide.h" - -namespace dart { -namespace collision { - -DARTCollisionDetector::DARTCollisionDetector() - : CollisionDetector() { -} - -DARTCollisionDetector::~DARTCollisionDetector() { -} - -CollisionNode* DARTCollisionDetector::createCollisionNode( - dynamics::BodyNode* _bodyNode) { - return new CollisionNode(_bodyNode); -} - -bool DARTCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, - bool /*_calculateContactPoints*/) { - clearAllContacts(); - - // Set all the body nodes are not in colliding - for (size_t i = 0; i < mCollisionNodes.size(); i++) - mCollisionNodes[i]->getBodyNode()->setColliding(false); - - std::vector contacts; - - for (size_t i = 0; i < mCollisionNodes.size(); i++) { - for (size_t j = i + 1; j < mCollisionNodes.size(); j++) { - CollisionNode* collNode1 = mCollisionNodes[i]; - CollisionNode* collNode2 = mCollisionNodes[j]; - dynamics::BodyNode* BodyNode1 = collNode1->getBodyNode(); - dynamics::BodyNode* BodyNode2 = collNode2->getBodyNode(); - - if (!isCollidable(collNode1, collNode2)) - continue; - - for (size_t k = 0; k < BodyNode1->getNumCollisionShapes(); k++) { - for (size_t l = 0; l < BodyNode2->getNumCollisionShapes(); l++) { - int currContactNum = mContacts.size(); - - contacts.clear(); - collide(BodyNode1->getCollisionShape(k), - BodyNode1->getTransform() - * BodyNode1->getCollisionShape(k)->getLocalTransform(), - BodyNode2->getCollisionShape(l), - BodyNode2->getTransform() - * BodyNode2->getCollisionShape(l)->getLocalTransform(), - &contacts); - - size_t numContacts = contacts.size(); - - for (unsigned int m = 0; m < numContacts; ++m) { - Contact contactPair; - contactPair = contacts[m]; - contactPair.bodyNode1 = BodyNode1; - contactPair.bodyNode2 = BodyNode2; - assert(contactPair.bodyNode1.lock() != nullptr); - assert(contactPair.bodyNode2.lock() != nullptr); - - mContacts.push_back(contactPair); - } - - std::vector markForDeletion(numContacts, false); - for (size_t m = 0; m < numContacts; m++) { - for (size_t n = m + 1; n < numContacts; n++) { - Eigen::Vector3d diff = - mContacts[currContactNum + m].point - - mContacts[currContactNum + n].point; - if (diff.dot(diff) < 1e-6) { - markForDeletion[m] = true; - break; - } - } - } - - for (int m = numContacts - 1; m >= 0; m--) - { - if (markForDeletion[m]) - mContacts.erase(mContacts.begin() + currContactNum + m); - } - } - } - } - } - - for (size_t i = 0; i < mContacts.size(); ++i) - { - // Set these two bodies are in colliding - mContacts[i].bodyNode1.lock()->setColliding(true); - mContacts[i].bodyNode2.lock()->setColliding(true); - } - - return !mContacts.empty(); -} - -bool DARTCollisionDetector::detectCollision(CollisionNode* _collNode1, - CollisionNode* _collNode2, - bool /*_calculateContactPoints*/) { - std::vector contacts; - dynamics::BodyNode* BodyNode1 = _collNode1->getBodyNode(); - dynamics::BodyNode* BodyNode2 = _collNode2->getBodyNode(); - - for (size_t i = 0; i < BodyNode1->getNumCollisionShapes(); i++) { - for (size_t j = 0; j < BodyNode2->getNumCollisionShapes(); j++) { - collide(BodyNode1->getCollisionShape(i), - BodyNode1->getTransform() - * BodyNode1->getCollisionShape(i)->getLocalTransform(), - BodyNode2->getCollisionShape(j), - BodyNode2->getTransform() - * BodyNode2->getCollisionShape(j)->getLocalTransform(), - &contacts); - } - } - - return contacts.size() > 0 ? true : false; -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/dart/DARTCollisionGroupData.cpp b/dart/collision/dart/DARTCollisionGroupData.cpp new file mode 100644 index 0000000000000..602f3c97e6d0a --- /dev/null +++ b/dart/collision/dart/DARTCollisionGroupData.cpp @@ -0,0 +1,97 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/dart/DARTCollisionGroupData.h" + +#include "dart/collision/CollisionObject.h" + +namespace dart { +namespace collision { + +//============================================================================== +DARTCollisionGroupData::DARTCollisionGroupData( + Engine* engine, + CollisionGroup* parent, + const CollisionObjectPtrs& /*collObjects*/) + : CollisionGroupData(engine, parent) +{ + // Do nothing +} + +//============================================================================== +std::unique_ptr +DARTCollisionGroupData::clone( + CollisionGroup* newCollisionGroup, + const CollisionObjectPtrs& collObjects) const +{ + return std::unique_ptr( + new DARTCollisionGroupData(mEngine, newCollisionGroup, collObjects)); +} + +//============================================================================== +void DARTCollisionGroupData::init() +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionGroupData::addCollisionObject( + const CollisionObjectPtr& /*object*/, const bool /*init*/) +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionGroupData::removeCollisionObject( + const CollisionObjectPtr& /*object*/, const bool /*init*/) +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionGroupData::removeAllCollisionObjects(bool /*init*/) +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionGroupData::update() +{ + // Do nothing +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/dart/DARTCollisionGroupData.h b/dart/collision/dart/DARTCollisionGroupData.h new file mode 100644 index 0000000000000..735ef99263111 --- /dev/null +++ b/dart/collision/dart/DARTCollisionGroupData.h @@ -0,0 +1,84 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_FCL_DARTCOLLISIONGROUPDATA_H_ +#define DART_COLLISION_FCL_DARTCOLLISIONGROUPDATA_H_ + +#include "dart/collision/CollisionGroup.h" +#include "dart/collision/CollisionGroupData.h" + +namespace dart { +namespace collision { + +class DARTCollisionObject; + +class DARTCollisionGroupData : public CollisionGroupData +{ +public: + + /// Constructor + DARTCollisionGroupData(Engine* engine, + CollisionGroup* parent, + const CollisionObjectPtrs& collObjects); + + // Documentation inherited + std::unique_ptr clone( + CollisionGroup* newCollisionGroup, + const CollisionObjectPtrs& collObjects) const override; + + // Documentation inherited + void init() override; + + // Documentation inherited + void addCollisionObject(const CollisionObjectPtr& object, + const bool init) override; + + // Documentation inherited + void removeCollisionObject(const CollisionObjectPtr& object, + const bool init) override; + + // Documentation inherited + void removeAllCollisionObjects(bool init) override; + + // Documentation inherited + void update() override; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_FCL_DARTCOLLISIONGROUPDATA_H_ diff --git a/dart/collision/dart/DARTCollisionObjectData.cpp b/dart/collision/dart/DARTCollisionObjectData.cpp new file mode 100644 index 0000000000000..b69f325c20fc9 --- /dev/null +++ b/dart/collision/dart/DARTCollisionObjectData.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/dart/DARTCollisionObjectData.h" + +#include +#include +#include + +#include "dart/common/Console.h" +#include "dart/collision/Engine.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl/FCLTypes.h" +#include "dart/collision/dart/DARTCollisionObjectData.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" + +namespace dart { +namespace collision { + +//============================================================================== +DARTCollisionObjectData::DARTCollisionObjectData( + Engine* engine, + CollisionObject* parent) + : CollisionObjectData(engine, parent) +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionObjectData::updateTransform(const Eigen::Isometry3d& /*tf*/) +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionObjectData::updateShape(const dynamics::ShapePtr& /*shape*/) +{ + // Do nothing +} + +//============================================================================== +void DARTCollisionObjectData::update() +{ + // Do nothing +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionObjectData.h similarity index 69% rename from dart/collision/dart/DARTCollisionDetector.h rename to dart/collision/dart/DARTCollisionObjectData.h index 748dd7aac083d..23521dc35a258 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionObjectData.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2013-2015, Georgia Tech Research Corporation + * Copyright (c) 2016, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Jeongseok Lee @@ -34,38 +34,42 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ -#define DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ +#ifndef DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ +#define DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ -#include "dart/collision/CollisionDetector.h" +#include +#include + +#include + +#include "dart/collision/CollisionObjectData.h" namespace dart { namespace collision { -/// \brief -class DARTCollisionDetector : public CollisionDetector { +class CollisionObject; + +class DARTCollisionObjectData : public CollisionObjectData +{ public: - /// \brief Default constructor - DARTCollisionDetector(); - /// \brief Default destructor - virtual ~DARTCollisionDetector(); + /// Constructor + DARTCollisionObjectData(Engine* engine, CollisionObject* parent); // Documentation inherited - virtual CollisionNode* createCollisionNode(dynamics::BodyNode* _bodyNode); + void updateTransform(const Eigen::Isometry3d& tf) override; // Documentation inherited - virtual bool detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints); + void updateShape(const dynamics::ShapePtr& shape) override; -protected: // Documentation inherited - virtual bool detectCollision(CollisionNode* _collNode1, - CollisionNode* _collNode2, - bool _calculateContactPoints); + void update() override; + +protected: + }; } // namespace collision } // namespace dart -#endif // DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ +#endif // DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ diff --git a/dart/collision/dart/DARTEngine.cpp b/dart/collision/dart/DARTEngine.cpp new file mode 100644 index 0000000000000..33eddeb6b999f --- /dev/null +++ b/dart/collision/dart/DARTEngine.cpp @@ -0,0 +1,291 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/dart/DARTEngine.h" + +#include "dart/collision/CollisionObject.h" +#include "dart/collision/dart/DARTCollide.h" +#include "dart/collision/dart/DARTCollisionObjectData.h" +#include "dart/collision/dart/DARTCollisionGroupData.h" + +namespace dart { +namespace collision { + +namespace { + +bool checkPair(CollisionObject* o1, CollisionObject* o2, + Result& result); + +bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, + double tol); + +void postProcess(CollisionObject* o1, CollisionObject* o2, Result& result); + +} // anonymous namespace + + + +//============================================================================== +DARTEnginePtr DARTEngine::create() +{ + return DARTEnginePtr(new DARTEngine()); +} + +//============================================================================== +DARTEngine::DARTEngine() +{ + // Do nothing +} + +//============================================================================== +DARTEngine::~DARTEngine() +{ + // Do nothing +} + +//============================================================================== +const std::string& DARTEngine::getTypeStatic() +{ + static const std::string& type("DART"); + return type; +} + +//============================================================================== +const std::string& DARTEngine::getType() const +{ + return getTypeStatic(); +} + +//============================================================================== +std::unique_ptr DARTEngine::createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& /*shape*/) +{ + return std::unique_ptr( + new DARTCollisionObjectData(this, parent)); +} + +//============================================================================== +std::unique_ptr DARTEngine::createCollisionGroupData( + CollisionGroup* parent, + const CollisionObjectPtrs& collObjects) +{ + return std::unique_ptr( + new DARTCollisionGroupData(this, parent, collObjects)); +} + +//============================================================================== +bool DARTEngine::detect( + CollisionObjectData* objectData1, + CollisionObjectData* objectData2, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(objectData1->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(objectData2->getEngine()->getType() == DARTEngine::getTypeStatic()); + + auto collObj1 = objectData1->getCollisionObject(); + auto collObj2 = objectData2->getCollisionObject(); + + return checkPair(collObj1, collObj2, result); +} + +//============================================================================== +bool DARTEngine::detect( + CollisionObjectData* objectData, + CollisionGroupData* groupData, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(objectData); + assert(groupData); + assert(objectData->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(groupData->getEngine()->getType() == DARTEngine::getTypeStatic()); + + auto collObj1 = objectData->getCollisionObject(); + auto collGrp = groupData->getCollisionGroup(); + auto collObjs2 = collGrp->getCollisionObjects(); + + for (auto collObj2 : collObjs2) + checkPair(collObj1, collObj2.get(), result); + + return !result.contacts.empty(); +} + +//============================================================================== +bool DARTEngine::detect(CollisionGroupData* groupData, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(groupData); + assert(groupData->getEngine()->getType() == DARTEngine::getTypeStatic()); + + auto collGrp = groupData->getCollisionGroup(); + auto collObjs = collGrp->getCollisionObjects(); + + if (collObjs.empty()) + return false; + + for (auto i = 0u; i < collObjs.size() - 1; ++i) + { + auto collObj1 = collObjs[i]; + + for (auto j = i + 1u; j < collObjs.size(); ++j) + { + auto collObj2 = collObjs[j]; + + checkPair(collObj1.get(), collObj2.get(), result); + } + } + + return !result.contacts.empty(); +} + +//============================================================================== +bool DARTEngine::detect(CollisionGroupData* groupData1, + CollisionGroupData* groupData2, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(groupData1); + assert(groupData2); + assert(groupData1->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(groupData2->getEngine()->getType() == DARTEngine::getTypeStatic()); + + auto collGrp1 = groupData1->getCollisionGroup(); + auto collGrp2 = groupData2->getCollisionGroup(); + + auto collObjs1 = collGrp1->getCollisionObjects(); + auto collObjs2 = collGrp2->getCollisionObjects(); + + if (collObjs1.empty() || collObjs2.empty()) + return false; + + for (auto i = 0u; i < collObjs1.size(); ++i) + { + auto collObj1 = collObjs1[i]; + + for (auto j = 0u; j < collObjs2.size(); ++j) + { + auto collObj2 = collObjs2[j]; + + checkPair(collObj1.get(), collObj2.get(), result); + } + } + + return !result.contacts.empty(); +} + + + +namespace { + +//============================================================================== +bool checkPair(CollisionObject* o1, CollisionObject* o2, Result& result) +{ + // TODO(JS): filtering + + // Perform narrow-phase detection + auto colliding = collide(o1->getShape(), o1->getTransform(), + o2->getShape(), o2->getTransform(), + &result.contacts); + + postProcess(o1, o2, result); + + return colliding != 0; +} + +//============================================================================== +bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, + double tol) +{ + return (pos1 - pos2).norm() < tol; +} + +//============================================================================== +void postProcess(CollisionObject* o1, CollisionObject* o2, + Result& result) +{ + if (result.contacts.empty()) + return; + + // Remove all the repeated points + const auto tol = 3.0e-12; + auto i = result.contacts.begin(); + while (i != result.contacts.end() - 1) + { + for (auto j = i + 1; j != result.contacts.end(); ++j) + { + if (isClose(i->point, j->point, tol)) + { + i = result.contacts.erase(i); + break; + } + } + + ++i; + } + + for (auto& contact : result.contacts) + { + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; + } + + std::cout << "=================" << std::endl; + std::cout << "# of contacts:" << result.contacts.size() << std::endl; + std::cout << "=================" << std::endl; + for (auto i = 0u; i < result.contacts.size(); ++i) + { + auto contact = result.contacts[i]; + + std::cout << "Contact (" << i << ")" << std::endl; + std::cout << "Point : " << contact.point.transpose() << std::endl; + std::cout << "Normal: " << contact.normal.transpose() << std::endl; + std::cout << "Depth : " << contact.penetrationDepth << std::endl; + std::cout << std::endl; + } + std::cout << "=================" << std::endl; +} + +} // anonymous namespace + +} // namespace collision +} // namespace dart diff --git a/dart/collision/dart/DARTEngine.h b/dart/collision/dart/DARTEngine.h new file mode 100644 index 0000000000000..90ce662dffabd --- /dev/null +++ b/dart/collision/dart/DARTEngine.h @@ -0,0 +1,97 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_DART_DARTENGINE_H_ +#define DART_COLLISION_DART_DARTENGINE_H_ + +#include + +#include "dart/collision/Engine.h" + +namespace dart { +namespace collision { + +/// FCL Collision detection engine +class DARTEngine : public Engine +{ +public: + + static DARTEnginePtr create(); + + /// Constructor + DARTEngine(); + + /// Constructor + virtual ~DARTEngine(); + + /// Return engine type "FCL" + static const std::string& getTypeStatic(); + + // Documentation inherit + const std::string& getType() const override; + + // Documentation inherit + std::unique_ptr createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) override; + + // Documentation inherit + std::unique_ptr createCollisionGroupData( + CollisionGroup* parent, + const CollisionObjectPtrs& collObjects) override; + + // Documentation inherit + bool detect(CollisionObjectData* object1, CollisionObjectData* object2, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionObjectData* object, CollisionGroupData* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroupData* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + const Option& option, Result& result) override; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_DART_DARTENGINE_H_ diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 97183feed5ccc..72ccbbac4c74a 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -37,18 +37,18 @@ #include "dart/constraint/ConstraintSolver.h" #include "dart/common/Console.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/SoftBodyNode.h" -#include "dart/dynamics/Joint.h" -#include "dart/dynamics/Skeleton.h" -#include "dart/dynamics/CollisionDetector.h" +#include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLEngine.h" #include "dart/collision/fcl_mesh/FCLMeshEngine.h" -#include "dart/collision/CollisionGroup.h" -#include "dart/collision/dart/DARTCollisionDetector.h" +#include "dart/collision/dart/DARTEngine.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" #endif +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/SoftBodyNode.h" +#include "dart/dynamics/Joint.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/CollisionDetector.h" #include "dart/constraint/ConstrainedGroup.h" #include "dart/constraint/ContactConstraint.h" #include "dart/constraint/SoftContactConstraint.h" @@ -216,8 +216,7 @@ void ConstraintSolver::setCollisionDetector( } //============================================================================== -collision::CollisionDetectorPtr -ConstraintSolver::getCollisionDetector() const +collision::CollisionDetectorPtr ConstraintSolver::getCollisionDetector() const { return mCollisionDetector; } @@ -530,16 +529,24 @@ void ConstraintSolver::solveConstrainedGroups() //============================================================================== bool ConstraintSolver::isSoftContact(const collision::Contact& _contact) const { + assert(dynamic_cast( + _contact.collisionObject1)); + assert(dynamic_cast( + _contact.collisionObject2)); + auto shapeFrameCollObj1 = static_cast( _contact.collisionObject1); auto shapeFrameCollObj2 = static_cast( _contact.collisionObject2); + auto bodyNode1 = shapeFrameCollObj1->getBodyNode().get(); + auto bodyNode2 = shapeFrameCollObj2->getBodyNode().get(); + auto bodyNode1IsSoft = - dynamic_cast(shapeFrameCollObj1) != nullptr; + dynamic_cast(bodyNode1) != nullptr; auto bodyNode2IsSoft = - dynamic_cast(shapeFrameCollObj2) != nullptr; + dynamic_cast(bodyNode2) != nullptr; return bodyNode1IsSoft || bodyNode2IsSoft; } diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 363d84a1175b1..fd57724ff6443 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -147,11 +147,12 @@ void SimWindow::draw() { } } else { if (mShowMarkers) { - collision::CollisionDetector* cd = - mWorld->getConstraintSolver()->getCollisionDetector(); - for (size_t k = 0; k < cd->getNumContacts(); k++) { - Eigen::Vector3d v = cd->getContact(k).point; - Eigen::Vector3d f = cd->getContact(k).force / 10.0; + const auto result = + mWorld->getConstraintSolver()->getLastCollisionResult(); + const auto& contacts = result.contacts; + for (size_t k = 0; k < contacts.size(); k++) { + Eigen::Vector3d v = contacts[k].point; + Eigen::Vector3d f = contacts[k].force / 10.0; glBegin(GL_LINES); glVertex3f(v[0], v[1], v[2]); glVertex3f(v[0] + f[0], v[1] + f[1], v[2] + f[2]); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 4deecc9baacca..f3c0197c80e7f 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -50,6 +50,7 @@ #include "dart/integration/SemiImplicitEulerIntegrator.h" #include "dart/dynamics/Skeleton.h" #include "dart/constraint/ConstraintSolver.h" +#include "dart/collision/CollisionGroup.h" namespace dart { namespace simulation { @@ -488,10 +489,17 @@ std::set World::removeAllSimpleFrames() } //============================================================================== -bool World::checkCollision(bool _checkAllCollisions) +bool World::checkCollision(bool checkAllCollisions) { - return mConstraintSolver->getCollisionDetector()->detectCollision( - _checkAllCollisions, false); + collision::Option option; + if (checkAllCollisions) + option.enableContact = true; + else + option.enableContact = false; + + collision::Result result; + + return mConstraintSolver->getCollisionGroup()->detect(option, result); } //============================================================================== @@ -503,22 +511,24 @@ constraint::ConstraintSolver* World::getConstraintSolver() const //============================================================================== void World::bake() { - collision::CollisionDetector* cd - = getConstraintSolver()->getCollisionDetector(); - int nContacts = cd->getNumContacts(); - int nSkeletons = getNumSkeletons(); + const auto collisionResult = getConstraintSolver()->getLastCollisionResult(); + const auto nContacts = static_cast(collisionResult.contacts.size()); + const auto nSkeletons = getNumSkeletons(); + Eigen::VectorXd state(getIndex(nSkeletons) + 6 * nContacts); - for (size_t i = 0; i < getNumSkeletons(); i++) + for (auto i = 0u; i < getNumSkeletons(); ++i) { state.segment(getIndex(i), getSkeleton(i)->getNumDofs()) = getSkeleton(i)->getPositions(); } - for (int i = 0; i < nContacts; i++) + + for (auto i = 0; i < nContacts; ++i) { - int begin = getIndex(nSkeletons) + i * 6; - state.segment(begin, 3) = cd->getContact(i).point; - state.segment(begin + 3, 3) = cd->getContact(i).force; + auto begin = getIndex(nSkeletons) + i * 6; + state.segment(begin, 3) = collisionResult.contacts[i].point; + state.segment(begin + 3, 3) = collisionResult.contacts[i].force; } + mRecording->addState(state); } diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index cfc26a0502789..d0216e7677140 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -45,7 +45,7 @@ #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" #endif -#include "dart/collision/dart/DARTCollisionDetector.h" +#include "dart/collision/dart/DARTEngine.h" #include "dart/collision/fcl/FCLEngine.h" #include "dart/collision/fcl_mesh/FCLMeshEngine.h" #include "dart/constraint/ConstraintSolver.h" @@ -263,11 +263,11 @@ simulation::WorldPtr SkelParser::readWorld( newWorld->getConstraintSolver()->setCollisionDetector( collision::CollisionDetector::create()); } -// else if (strCD == "dart") -// { -// newWorld->getConstraintSolver()->setCollisionEngine( -// new collision::DARTEngine()); -// } + else if (strCD == "dart") + { + newWorld->getConstraintSolver()->setCollisionDetector( + collision::CollisionDetector::create()); + } //#ifdef HAVE_BULLET_COLLISION // else if (strCD == "bullet") // { diff --git a/dart/utils/sdf/SoftSdfParser.cpp b/dart/utils/sdf/SoftSdfParser.cpp index df3bc2c2703c8..d2dbc2f522641 100644 --- a/dart/utils/sdf/SoftSdfParser.cpp +++ b/dart/utils/sdf/SoftSdfParser.cpp @@ -41,7 +41,6 @@ #include #include "dart/common/Console.h" -#include "dart/collision/dart/DARTCollisionDetector.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/CylinderShape.h" diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index a90f0ff3deb87..ec55ef50d4a46 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -213,9 +213,9 @@ class MyWindow : public dart::gui::SimWindow // Look through the collisions to see if the new object would start in // collision with something - auto collisionEngine = mWorld->getConstraintSolver()->getCollisionEngine(); + auto collisionEngine = mWorld->getConstraintSolver()->getCollisionDetector(); auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); - auto newGroup = createShapeNodeCollisionGroup(collisionEngine, object); + auto newGroup = createShapeFrameCollisionGroup(collisionEngine, object); dart::collision::Option option; dart::collision::Result result; diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index c7a9a7d5f0079..fb4af8210e3b9 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -264,9 +264,9 @@ class MyWindow : public dart::gui::SimWindow // Look through the collisions to see if any dominoes are penetrating // something - auto collisionEngine = mWorld->getConstraintSolver()->getCollisionEngine(); + auto collisionEngine = mWorld->getConstraintSolver()->getCollisionDetector(); auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); - auto newGroup = createShapeNodeCollisionGroup(collisionEngine, newDomino); + auto newGroup = createShapeFrameCollisionGroup(collisionEngine, newDomino); dart::collision::Option option; dart::collision::Result result; diff --git a/unittests/testConstraint.cpp b/unittests/testConstraint.cpp index 8ddfaa791d9a7..686d74f97b582 100644 --- a/unittests/testConstraint.cpp +++ b/unittests/testConstraint.cpp @@ -45,7 +45,7 @@ #include "dart/common/Console.h" #include "dart/math/Geometry.h" #include "dart/math/Helpers.h" -#include "dart/collision/dart/DARTCollisionDetector.h" +#include "dart/collision/dart/DARTEngine.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" @@ -126,7 +126,7 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) world->setGravity(Vector3d(0.0, -10.00, 0.0)); world->setTimeStep(0.001); world->getConstraintSolver()->setCollisionDetector( - new DARTCollisionDetector()); + CollisionDetector::create()); SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0)); BodyNode* sphere = sphereSkel->getBodyNode(0); @@ -157,9 +157,6 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) EXPECT_EQ((int)world->getNumSkeletons(), 2); - ConstraintSolver* cs = world->getConstraintSolver(); - dart::collision::CollisionDetector* cd = cs->getCollisionDetector(); - // Lower and upper bound of configuration for system // double lb = -1.5 * DART_PI; // double ub = 1.5 * DART_PI; @@ -173,8 +170,7 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) // std::cout << "pos1:" << pos1.transpose() << std::endl; // std::cout << "vel1:" << vel1.transpose() << std::endl; - cd->detectCollision(true, true); - if (cd->getNumContacts() == 0) + if (!world->checkCollision()) { world->step(); continue; @@ -192,9 +188,12 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) world->step(); - for (size_t j = 0; j < cd->getNumContacts(); ++j) + const auto& result = world->getConstraintSolver()->getLastCollisionResult(); + const auto& contacts = result.contacts; + + for (size_t j = 0; j < contacts.size(); ++j) { - Contact contact = cd->getContact(j); + const Contact& contact = contacts[j]; Vector3d pos1 = sphere->getTransform().inverse() * contact.point; Vector3d vel1 = sphere->getLinearVelocity(pos1); From da625ecb9ed899bad4d47975e3b23844c25ecd73 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 26 Feb 2016 16:40:02 -0500 Subject: [PATCH 10/67] Fix bug of DARTEngine in postprocessing --- dart/collision/CollisionObject.h | 13 ----- .../collision/dart/DARTCollisionGroupData.cpp | 4 +- dart/collision/dart/DARTCollisionGroupData.h | 8 +-- .../dart/DARTCollisionObjectData.cpp | 18 ------- dart/collision/dart/DARTCollisionObjectData.h | 4 -- dart/collision/dart/DARTEngine.cpp | 53 ++++++++----------- dart/collision/dart/DARTEngine.h | 4 +- dart/collision/fcl/FCLCollisionGroupData.cpp | 9 ++-- dart/collision/fcl/FCLCollisionGroupData.h | 5 +- dart/collision/fcl/FCLCollisionObjectData.cpp | 5 +- dart/collision/fcl/FCLEngine.cpp | 17 +++--- dart/collision/fcl/FCLEngine.h | 3 +- .../fcl_mesh/FCLMeshCollisionGroupData.cpp | 6 ++- .../fcl_mesh/FCLMeshCollisionGroupData.h | 2 + .../fcl_mesh/FCLMeshCollisionObjectData.cpp | 2 +- dart/collision/fcl_mesh/FCLMeshEngine.cpp | 45 ++++++++++------ dart/collision/fcl_mesh/FCLMeshEngine.h | 1 + dart/constraint/ContactConstraint.cpp | 19 ++++--- dart/constraint/SoftContactConstraint.cpp | 5 ++ dart/dynamics/CollisionDetector.cpp | 8 ++- unittests/testCollision.cpp | 4 ++ 21 files changed, 117 insertions(+), 118 deletions(-) diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 8f4ae1e7c6431..a5a49e2105649 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -81,19 +81,6 @@ class CollisionObject void updateEngineData(); // TODO(JS): remove. instead, CollisionObjectData should be updated by engine - template - T* as() - { - return static_cast(this); - } - - template - const T* as() const - { - return static_cast(this); - } - // TODO(JS): Need this? - protected: /// Contructor diff --git a/dart/collision/dart/DARTCollisionGroupData.cpp b/dart/collision/dart/DARTCollisionGroupData.cpp index 602f3c97e6d0a..adf40a48a6d56 100644 --- a/dart/collision/dart/DARTCollisionGroupData.cpp +++ b/dart/collision/dart/DARTCollisionGroupData.cpp @@ -54,11 +54,11 @@ DARTCollisionGroupData::DARTCollisionGroupData( //============================================================================== std::unique_ptr DARTCollisionGroupData::clone( - CollisionGroup* newCollisionGroup, + CollisionGroup* newParent, const CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new DARTCollisionGroupData(mEngine, newCollisionGroup, collObjects)); + new DARTCollisionGroupData(mEngine, newParent, collObjects)); } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionGroupData.h b/dart/collision/dart/DARTCollisionGroupData.h index 735ef99263111..f468d52033db6 100644 --- a/dart/collision/dart/DARTCollisionGroupData.h +++ b/dart/collision/dart/DARTCollisionGroupData.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_DARTCOLLISIONGROUPDATA_H_ -#define DART_COLLISION_FCL_DARTCOLLISIONGROUPDATA_H_ +#ifndef DART_COLLISION_DART_DARTCOLLISIONGROUPDATA_H_ +#define DART_COLLISION_DART_DARTCOLLISIONGROUPDATA_H_ #include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionGroupData.h" @@ -56,7 +56,7 @@ class DARTCollisionGroupData : public CollisionGroupData // Documentation inherited std::unique_ptr clone( - CollisionGroup* newCollisionGroup, + CollisionGroup* newParent, const CollisionObjectPtrs& collObjects) const override; // Documentation inherited @@ -81,4 +81,4 @@ class DARTCollisionGroupData : public CollisionGroupData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_DARTCOLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_DART_DARTCOLLISIONGROUPDATA_H_ diff --git a/dart/collision/dart/DARTCollisionObjectData.cpp b/dart/collision/dart/DARTCollisionObjectData.cpp index b69f325c20fc9..17af59c2a582f 100644 --- a/dart/collision/dart/DARTCollisionObjectData.cpp +++ b/dart/collision/dart/DARTCollisionObjectData.cpp @@ -36,24 +36,6 @@ #include "dart/collision/dart/DARTCollisionObjectData.h" -#include -#include -#include - -#include "dart/common/Console.h" -#include "dart/collision/Engine.h" -#include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/dart/DARTCollisionObjectData.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/MeshShape.h" -#include "dart/dynamics/SoftMeshShape.h" - namespace dart { namespace collision { diff --git a/dart/collision/dart/DARTCollisionObjectData.h b/dart/collision/dart/DARTCollisionObjectData.h index 23521dc35a258..eb49052c5fcab 100644 --- a/dart/collision/dart/DARTCollisionObjectData.h +++ b/dart/collision/dart/DARTCollisionObjectData.h @@ -37,11 +37,7 @@ #ifndef DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ #define DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ -#include #include - -#include - #include "dart/collision/CollisionObjectData.h" namespace dart { diff --git a/dart/collision/dart/DARTEngine.cpp b/dart/collision/dart/DARTEngine.cpp index 33eddeb6b999f..8ab3cf8ac88c5 100644 --- a/dart/collision/dart/DARTEngine.cpp +++ b/dart/collision/dart/DARTEngine.cpp @@ -36,6 +36,7 @@ #include "dart/collision/dart/DARTEngine.h" +#include #include "dart/collision/CollisionObject.h" #include "dart/collision/dart/DARTCollide.h" #include "dart/collision/dart/DARTCollisionObjectData.h" @@ -52,7 +53,8 @@ bool checkPair(CollisionObject* o1, CollisionObject* o2, bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, double tol); -void postProcess(CollisionObject* o1, CollisionObject* o2, Result& result); +void postProcess(CollisionObject* o1, CollisionObject* o2, + Result& totalResult, const Result& pairResult); } // anonymous namespace @@ -222,12 +224,14 @@ bool checkPair(CollisionObject* o1, CollisionObject* o2, Result& result) { // TODO(JS): filtering + Result pairResult; + // Perform narrow-phase detection auto colliding = collide(o1->getShape(), o1->getTransform(), o2->getShape(), o2->getTransform(), - &result.contacts); + &pairResult.contacts); - postProcess(o1, o2, result); + postProcess(o1, o2, result, pairResult); return colliding != 0; } @@ -241,48 +245,35 @@ bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, //============================================================================== void postProcess(CollisionObject* o1, CollisionObject* o2, - Result& result) + Result& totalResult, const Result& pairResult) { - if (result.contacts.empty()) + if (pairResult.contacts.empty()) return; // Remove all the repeated points const auto tol = 3.0e-12; - auto i = result.contacts.begin(); - while (i != result.contacts.end() - 1) + auto i = pairResult.contacts.begin(); + while (i != pairResult.contacts.end() - 1) { - for (auto j = i + 1; j != result.contacts.end(); ++j) + for (auto j = i + 1; j != pairResult.contacts.end(); ++j) { if (isClose(i->point, j->point, tol)) - { - i = result.contacts.erase(i); break; - } } - ++i; - } + totalResult.contacts.push_back(*i); + totalResult.contacts.back().collisionObject1 = o1; + totalResult.contacts.back().collisionObject2 = o2; - for (auto& contact : result.contacts) - { - contact.collisionObject1 = o1; - contact.collisionObject2 = o2; + ++i; } - std::cout << "=================" << std::endl; - std::cout << "# of contacts:" << result.contacts.size() << std::endl; - std::cout << "=================" << std::endl; - for (auto i = 0u; i < result.contacts.size(); ++i) - { - auto contact = result.contacts[i]; - - std::cout << "Contact (" << i << ")" << std::endl; - std::cout << "Point : " << contact.point.transpose() << std::endl; - std::cout << "Normal: " << contact.normal.transpose() << std::endl; - std::cout << "Depth : " << contact.penetrationDepth << std::endl; - std::cout << std::endl; - } - std::cout << "=================" << std::endl; +// for (auto& contact : pairResult.contacts) +// { +// totalResult.contacts.push_back(contact); +// totalResult.contacts.back().collisionObject1 = o1; +// totalResult.contacts.back().collisionObject2 = o2; +// } } } // anonymous namespace diff --git a/dart/collision/dart/DARTEngine.h b/dart/collision/dart/DARTEngine.h index 90ce662dffabd..4a7ebcb8adb63 100644 --- a/dart/collision/dart/DARTEngine.h +++ b/dart/collision/dart/DARTEngine.h @@ -44,7 +44,7 @@ namespace dart { namespace collision { -/// FCL Collision detection engine +/// DART Collision detection engine class DARTEngine : public Engine { public: @@ -57,7 +57,7 @@ class DARTEngine : public Engine /// Constructor virtual ~DARTEngine(); - /// Return engine type "FCL" + /// Return engine type "DART" static const std::string& getTypeStatic(); // Documentation inherit diff --git a/dart/collision/fcl/FCLCollisionGroupData.cpp b/dart/collision/fcl/FCLCollisionGroupData.cpp index fca170d3d47a8..36c03ec485c0d 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.cpp +++ b/dart/collision/fcl/FCLCollisionGroupData.cpp @@ -45,8 +45,9 @@ namespace collision { //============================================================================== FCLCollisionGroupData::FCLCollisionGroupData( Engine* engine, + CollisionGroup* parent, const FCLCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(engine), + : CollisionGroupData(engine, parent), mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { for (auto collObj : collObjects) @@ -60,10 +61,12 @@ FCLCollisionGroupData::FCLCollisionGroupData( //============================================================================== std::unique_ptr -FCLCollisionGroupData::clone(const CollisionObjectPtrs& collObjects) const +FCLCollisionGroupData::clone( + CollisionGroup* newParent, + const CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new FCLCollisionGroupData(mEngine, collObjects)); + new FCLCollisionGroupData(mEngine, newParent, collObjects)); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionGroupData.h b/dart/collision/fcl/FCLCollisionGroupData.h index ec430465db8d3..4ae17462f9553 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.h +++ b/dart/collision/fcl/FCLCollisionGroupData.h @@ -56,10 +56,13 @@ class FCLCollisionGroupData : public CollisionGroupData using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLCollisionGroupData(Engine* engine, const CollisionObjects& collObjects); + FCLCollisionGroupData(Engine* engine, + CollisionGroup* parent, + const CollisionObjects& collObjects); // Documentation inherited std::unique_ptr clone( + CollisionGroup* newParent, const CollisionObjectPtrs& collObjects) const override; // Documentation inherited diff --git a/dart/collision/fcl/FCLCollisionObjectData.cpp b/dart/collision/fcl/FCLCollisionObjectData.cpp index 1a02574d5783b..de7bf7a6c2b6f 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectData.cpp @@ -608,10 +608,11 @@ FCLCollisionObjectUserData::FCLCollisionObjectUserData( } //============================================================================== -FCLCollisionObjectData::FCLCollisionObjectData(Engine* engine, +FCLCollisionObjectData::FCLCollisionObjectData( + Engine* engine, CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectData(engine), + : CollisionObjectData(engine, parent), mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), mFCLCollisionObject(new fcl::CollisionObject( createFCLCollisionGeometry(shape))) diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLEngine.cpp index 467e5490165de..7632dc9ee3a0b 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLEngine.cpp @@ -51,7 +51,7 @@ namespace collision { namespace { -bool collisionCallBack(fcl::CollisionObject* o1, +bool checkPair(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); @@ -144,10 +144,11 @@ std::unique_ptr FCLEngine::createCollisionObjectData( //============================================================================== std::unique_ptr FCLEngine::createCollisionGroupData( + CollisionGroup* parent, const CollisionObjectPtrs& collObjects) { return std::unique_ptr( - new FCLCollisionGroupData(this, collObjects)); + new FCLCollisionGroupData(this, parent, collObjects)); } //============================================================================== @@ -168,7 +169,7 @@ bool FCLEngine::detect( auto fclCollObj2 = castedData2->getFCLCollisionObject(); FCLCollisionData collData(&option, &result); - collisionCallBack(fclCollObj1, fclCollObj2, &collData); + checkPair(fclCollObj1, fclCollObj2, &collData); return !result.contacts.empty(); } @@ -193,7 +194,7 @@ bool FCLEngine::detect( auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); + broadPhaseAlg->collide(fclObject, &collData, checkPair); return !result.contacts.empty(); } @@ -212,7 +213,7 @@ bool FCLEngine::detect(CollisionGroupData* groupData, auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(&collData, collisionCallBack); + broadPhaseAlg->collide(&collData, checkPair); return !result.contacts.empty(); } @@ -236,7 +237,7 @@ bool FCLEngine::detect(CollisionGroupData* groupData1, auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); - broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, checkPair); return !result.contacts.empty(); } @@ -246,9 +247,7 @@ bool FCLEngine::detect(CollisionGroupData* groupData1, namespace { //============================================================================== -bool collisionCallBack(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata) +bool checkPair(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { FCLCollisionData* collData = static_cast(cdata); diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/fcl/FCLEngine.h index 9bbec26cbdc4a..deaf137783313 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/fcl/FCLEngine.h @@ -44,8 +44,6 @@ namespace dart { namespace collision { -class FCLCollisionGroup; - /// FCL Collision detection engine class FCLEngine : public Engine { @@ -72,6 +70,7 @@ class FCLEngine : public Engine // Documentation inherit std::unique_ptr createCollisionGroupData( + CollisionGroup* parent, const CollisionObjectPtrs& collObjects) override; // Documentation inherit diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp index 4119ff18184c0..129058c6f3c09 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp @@ -45,8 +45,9 @@ namespace collision { //============================================================================== FCLMeshCollisionGroupData::FCLMeshCollisionGroupData( Engine* engine, + CollisionGroup* parent, const FCLMeshCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(engine), + : CollisionGroupData(engine, parent), mBroadPhaseAlg(new FCLCollisionManager()) { for (auto collObj : collObjects) @@ -61,10 +62,11 @@ FCLMeshCollisionGroupData::FCLMeshCollisionGroupData( //============================================================================== std::unique_ptr FCLMeshCollisionGroupData::clone( + CollisionGroup* newParent, const CollisionGroupData::CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new FCLMeshCollisionGroupData(mEngine, collObjects)); + new FCLMeshCollisionGroupData(mEngine, newParent, collObjects)); } //============================================================================== diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h index 9bbf9309c0fb7..c53393ca2f5d1 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h @@ -59,10 +59,12 @@ class FCLMeshCollisionGroupData : public CollisionGroupData /// Constructor FCLMeshCollisionGroupData(Engine* engine, + CollisionGroup* parent, const CollisionObjects& collObjects); // Documentation inherited std::unique_ptr clone( + CollisionGroup* newParent, const CollisionObjectPtrs& collObjects) const override; // Documentation inherited diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp index e591aec1b1efc..539f36a15b942 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp @@ -566,7 +566,7 @@ FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( Engine* engine, CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectData(engine), + : CollisionObjectData(engine, parent), mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), mFCLCollisionObject(new fcl::CollisionObject( createFCLCollisionGeometry(shape))) diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.cpp b/dart/collision/fcl_mesh/FCLMeshEngine.cpp index 129a868350e3c..5a5ddf6de3af9 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.cpp +++ b/dart/collision/fcl_mesh/FCLMeshEngine.cpp @@ -54,7 +54,7 @@ namespace collision { namespace { -bool collisionCallBack(fcl::CollisionObject* o1, +bool checkPair(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); @@ -110,6 +110,11 @@ int evalContactPosition( Eigen::Vector3d* contactPosition1, Eigen::Vector3d* contactPosition2); +bool isColinear(const Eigen::Vector3d& pos1, + const Eigen::Vector3d& pos2, + const Eigen::Vector3d& pos3, + double tol); + int FFtest( const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, @@ -165,10 +170,11 @@ FCLMeshEngine::createCollisionObjectData( //============================================================================== std::unique_ptr FCLMeshEngine::createCollisionGroupData( + CollisionGroup* parent, const CollisionObjectPtrs& collObjects) { return std::unique_ptr( - new FCLMeshCollisionGroupData(this, collObjects)); + new FCLMeshCollisionGroupData(this, parent, collObjects)); } //============================================================================== @@ -189,7 +195,7 @@ bool FCLMeshEngine::detect( auto fclCollObj2 = castedData2->getFCLCollisionObject(); FCLCollisionData collData(&option, &result); - collisionCallBack(fclCollObj1, fclCollObj2, &collData); + checkPair(fclCollObj1, fclCollObj2, &collData); return !result.contacts.empty(); } @@ -214,7 +220,7 @@ bool FCLMeshEngine::detect( auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(fclObject, &collData, collisionCallBack); + broadPhaseAlg->collide(fclObject, &collData, checkPair); return !result.contacts.empty(); } @@ -233,7 +239,7 @@ bool FCLMeshEngine::detect(CollisionGroupData* groupData, auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(&collData, collisionCallBack); + broadPhaseAlg->collide(&collData, checkPair); return !result.contacts.empty(); } @@ -257,7 +263,7 @@ bool FCLMeshEngine::detect(CollisionGroupData* groupData1, auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); FCLCollisionData collData(&option, &result); - broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallBack); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, checkPair); return !result.contacts.empty(); } @@ -267,7 +273,7 @@ bool FCLMeshEngine::detect(CollisionGroupData* groupData1, namespace { //============================================================================== -bool collisionCallBack(fcl::CollisionObject* o1, +bool checkPair(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { @@ -385,8 +391,7 @@ void postProcess(const fcl::CollisionResult& fclResult, - const double ZERO = 0.000001; - const double ZERO2 = ZERO*ZERO; + const double tol = 1e-12; auto unfilteredSize = unfiltered.size(); @@ -403,7 +408,7 @@ void postProcess(const fcl::CollisionResult& fclResult, const auto diff = contact1.point - contact2.point; - if (diff.norm() < 3.0 * ZERO2) + if (diff.norm() < 3.0 * tol) { markForDeletion[i] = true; break; @@ -433,11 +438,7 @@ void postProcess(const fcl::CollisionResult& fclResult, // const auto& contact3 = unfiltered[k]; -// const auto va = contact1.point - contact2.point; -// const auto vb = contact1.point - contact3.point; -// const auto v = va.cross(vb); - -// if (v.norm() < ZERO2) +// if (isColinear(contact1.point, contact2.point, contact3.point, tol)) // { // markForDeletion[i] = true; // break; @@ -573,6 +574,20 @@ double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) return area; } +//============================================================================== +bool isColinear(const Eigen::Vector3d& pos1, + const Eigen::Vector3d& pos2, + const Eigen::Vector3d& pos3, + double tol) +{ + const auto va = pos1 - pos2; + const auto vb = pos1 - pos3; + const auto v = va.cross(vb); + + return v.norm() < tol; +} + + //============================================================================== void convertOption(const Option& fclOption, fcl::CollisionRequest& request) { diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.h b/dart/collision/fcl_mesh/FCLMeshEngine.h index 5cbc1a5772ddf..485f33696187c 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.h +++ b/dart/collision/fcl_mesh/FCLMeshEngine.h @@ -72,6 +72,7 @@ class FCLMeshEngine : public Engine // Documentation inherit std::unique_ptr createCollisionGroupData( + CollisionGroup* parent, const CollisionObjectPtrs& collObjects) override; // Documentation inherit diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index b20516826f8ee..79128c60bde1d 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -80,6 +80,11 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, mIsBounceOn(false), mActive(false) { + assert(dynamic_cast( + _contact.collisionObject1)->getBodyNode()); + assert(dynamic_cast( + _contact.collisionObject2)->getBodyNode()); + // TODO(JS): Assumed single contact mContacts.push_back(&_contact); @@ -662,13 +667,13 @@ void ContactConstraint::applyImpulse(double* _lambda) for (size_t i = 0; i < mContacts.size(); ++i) { -// std::cout << "_lambda1: " << _lambda[_idx] << std::endl; -// std::cout << "_lambda2: " << _lambda[_idx + 1] << std::endl; -// std::cout << "_lambda3: " << _lambda[_idx + 2] << std::endl; +// std::cout << "_lambda1: " << _lambda[index] << std::endl; +// std::cout << "_lambda2: " << _lambda[index + 1] << std::endl; +// std::cout << "_lambda3: " << _lambda[index + 2] << std::endl; -// std::cout << "imp1: " << mJacobians2[i * 3 + 0] * _lambda[_idx] << std::endl; -// std::cout << "imp2: " << mJacobians2[i * 3 + 1] * _lambda[_idx + 1] << std::endl; -// std::cout << "imp3: " << mJacobians2[i * 3 + 2] * _lambda[_idx + 2] << std::endl; +// std::cout << "imp1: " << mJacobians2[i * 3 + 0] * _lambda[index] << std::endl; +// std::cout << "imp2: " << mJacobians2[i * 3 + 1] * _lambda[index + 1] << std::endl; +// std::cout << "imp3: " << mJacobians2[i * 3 + 2] * _lambda[index + 2] << std::endl; assert(!math::isNan(_lambda[index])); @@ -750,7 +755,7 @@ void ContactConstraint::getRelVelocity(double* _relVel) if (mBodyNode2->isReactive()) _relVel[i] -= mJacobians2[i].dot(mBodyNode2->getSpatialVelocity()); -// std::cout << "_relVel[i + _idx]: " << _relVel[i + _idx] << std::endl; +// std::cout << "_relVel[i]: " << _relVel[i] << std::endl; } } diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index 3dc0c3d03ded3..dfff4acc04e52 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -88,6 +88,11 @@ SoftContactConstraint::SoftContactConstraint( mIsBounceOn(false), mActive(false) { + assert(dynamic_cast( + _contact.collisionObject1)->getBodyNode()); + assert(dynamic_cast( + _contact.collisionObject2)->getBodyNode()); + // TODO(JS): Assumed single contact mContacts.push_back(&_contact); diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index 17151336732ee..95d7699867550 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -49,6 +49,10 @@ ShapeFrameCollisionObject::ShapeFrameCollisionObject( : collision::CollisionObject(collisionDetector, shape), mBodyNode(bodyNode) { + assert(collisionDetector); + assert(shape); + assert(bodyNode); + auto found = false; auto numShapes = mBodyNode->getNumCollisionShapes(); for (auto i = 0u; i < numShapes; ++i) @@ -108,8 +112,8 @@ std::vector createShapeFrameCollisionObjects( for (auto j = 0u; j < numColShapes; ++j) { auto shape = bodyNode->getCollisionShape(j); - auto collObj = collisionDetector->createCollisionObject< - ShapeFrameCollisionObject>(shape, bodyNode); + auto collObj = + createShapeFrameCollisionObject(collisionDetector, shape, bodyNode); objects.push_back( std::static_pointer_cast(collObj)); diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 323a74a720741..d77ab493abcaa 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -47,6 +47,7 @@ #include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLEngine.h" #include "dart/collision/fcl_mesh/FCLMeshEngine.h" +#include "dart/collision/dart/DARTEngine.h" #include "dart/dynamics/dynamics.h" #include "dart/simulation/simulation.h" #include "dart/utils/utils.h" @@ -569,6 +570,7 @@ TEST_F(COLLISION, FreeCollisionObjects) { testFreeCollisionObjects(); testFreeCollisionObjects(); + testFreeCollisionObjects(); } //============================================================================== @@ -607,6 +609,7 @@ TEST_F(COLLISION, BodyNodeNodes) { testBodyNodes(); testBodyNodes(); + testBodyNodes(); } //============================================================================== @@ -635,6 +638,7 @@ TEST_F(COLLISION, Skeletons) { testSkeletons(); testSkeletons(); + testSkeletons(); } //============================================================================== From c0b4dfec3b41c4bb40782e00c3c2bf1c30cc8c70 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 29 Feb 2016 16:49:02 -0500 Subject: [PATCH 11/67] Merge CollisionDetect and Engine, and implementing BulletCollisionDetector --- dart/collision/CollisionDetector.cpp | 46 +-- dart/collision/CollisionDetector.h | 63 +-- dart/collision/CollisionGroup.cpp | 46 +-- dart/collision/CollisionGroup.h | 19 +- dart/collision/CollisionGroupData.cpp | 16 +- dart/collision/CollisionGroupData.h | 15 +- dart/collision/CollisionObject.cpp | 27 +- dart/collision/CollisionObject.h | 14 +- dart/collision/CollisionObjectData.cpp | 15 +- dart/collision/CollisionObjectData.h | 9 +- dart/collision/Engine.cpp | 51 --- dart/collision/Engine.h | 97 ----- dart/collision/SmartPointer.h | 10 +- .../bullet/BulletCollisionDetector.cpp | 359 +++++++++++------- .../bullet/BulletCollisionDetector.h | 65 ++-- .../bullet/BulletCollisionGroupData.cpp | 190 +++++++++ .../bullet/BulletCollisionGroupData.h | 120 ++++++ dart/collision/bullet/BulletCollisionNode.cpp | 240 ------------ dart/collision/bullet/BulletCollisionNode.h | 100 ----- .../bullet/BulletCollisionObjectData.cpp | 268 +++++++++++++ .../bullet/BulletCollisionObjectData.h | 112 ++++++ ...RTEngine.cpp => DARTCollisionDetector.cpp} | 60 ++- .../DARTCollisionDetector.h} | 30 +- .../collision/dart/DARTCollisionGroupData.cpp | 13 +- dart/collision/dart/DARTCollisionGroupData.h | 6 +- .../dart/DARTCollisionObjectData.cpp | 4 +- dart/collision/dart/DARTCollisionObjectData.h | 2 +- dart/collision/detail/Engine.h | 48 --- ...FCLEngine.cpp => FCLCollisionDetector.cpp} | 85 ++--- .../FCLCollisionDetector.h} | 33 +- dart/collision/fcl/FCLCollisionGroupData.cpp | 23 +- dart/collision/fcl/FCLCollisionGroupData.h | 6 +- dart/collision/fcl/FCLCollisionObjectData.cpp | 11 +- dart/collision/fcl/FCLCollisionObjectData.h | 2 +- ...ngine.cpp => FCLMeshCollisionDetector.cpp} | 83 ++-- ...eshEngine.h => FCLMeshCollisionDetector.h} | 28 +- .../fcl_mesh/FCLMeshCollisionGroupData.cpp | 22 +- .../fcl_mesh/FCLMeshCollisionGroupData.h | 6 +- .../fcl_mesh/FCLMeshCollisionObjectData.cpp | 6 +- .../fcl_mesh/FCLMeshCollisionObjectData.h | 2 +- dart/constraint/ConstraintSolver.cpp | 9 +- dart/constraint/ConstraintSolver.h | 1 - dart/dynamics/CollisionDetector.h | 3 +- dart/utils/SkelParser.cpp | 31 +- unittests/testCollision.cpp | 64 ++-- unittests/testConstraint.cpp | 4 +- 46 files changed, 1374 insertions(+), 1090 deletions(-) delete mode 100644 dart/collision/Engine.cpp delete mode 100644 dart/collision/Engine.h create mode 100644 dart/collision/bullet/BulletCollisionGroupData.cpp create mode 100644 dart/collision/bullet/BulletCollisionGroupData.h delete mode 100644 dart/collision/bullet/BulletCollisionNode.cpp delete mode 100644 dart/collision/bullet/BulletCollisionNode.h create mode 100644 dart/collision/bullet/BulletCollisionObjectData.cpp create mode 100644 dart/collision/bullet/BulletCollisionObjectData.h rename dart/collision/dart/{DARTEngine.cpp => DARTCollisionDetector.cpp} (81%) rename dart/collision/{fcl/FCLEngine.h => dart/DARTCollisionDetector.h} (85%) delete mode 100644 dart/collision/detail/Engine.h rename dart/collision/fcl/{FCLEngine.cpp => FCLCollisionDetector.cpp} (82%) rename dart/collision/{dart/DARTEngine.h => fcl/FCLCollisionDetector.h} (83%) rename dart/collision/fcl_mesh/{FCLMeshEngine.cpp => FCLMeshCollisionDetector.cpp} (88%) rename dart/collision/fcl_mesh/{FCLMeshEngine.h => FCLMeshCollisionDetector.h} (87%) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 4e3dd3c3ea59d..183783d9fa4f8 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -42,7 +42,6 @@ #include #include "dart/common/Console.h" -#include "dart/collision/Engine.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/CollisionGroup.h" #include "dart/dynamics/BodyNode.h" @@ -51,24 +50,6 @@ namespace dart { namespace collision { -//============================================================================== -CollisionDetector::~CollisionDetector() -{ - // Do nothing -} - -//============================================================================== -Engine* CollisionDetector::getEngine() -{ - return mEngine.get(); -} - -//============================================================================== -const Engine* CollisionDetector::getEngine() const -{ - return mEngine.get(); -} - //============================================================================== std::shared_ptr CollisionDetector::createCollisionGroup( const std::vector& objects) @@ -76,12 +57,6 @@ std::shared_ptr CollisionDetector::createCollisionGroup( return std::make_shared(shared_from_this(), objects); } -//============================================================================== -std::shared_ptr CollisionDetector::createCollisionGroup() -{ - return std::make_shared(shared_from_this()); -} - //============================================================================== bool CollisionDetector::detect( CollisionObject* object1, CollisionObject* object2, @@ -92,8 +67,8 @@ bool CollisionDetector::detect( object1->updateEngineData(); object2->updateEngineData(); - return mEngine->detect(object1->getEngineData(), object2->getEngineData(), - option, result); + return detect(object1->getEngineData(), object2->getEngineData(), + option, result); } //============================================================================== @@ -106,8 +81,8 @@ bool CollisionDetector::detect( object->updateEngineData(); group->updateEngineData(); - return mEngine->detect(object->getEngineData(), group->getEngineData(), - option, result); + return detect(object->getEngineData(), group->getEngineData(), + option, result); } //============================================================================== @@ -124,7 +99,7 @@ bool CollisionDetector::detect( { group->updateEngineData(); - return mEngine->detect(group->getEngineData(), option, result); + return detect(group->getEngineData(), option, result); } //============================================================================== @@ -137,15 +112,16 @@ bool CollisionDetector::detect( group1->updateEngineData(); group2->updateEngineData(); - return mEngine->detect(group1->getEngineData(), group2->getEngineData(), - option, result); + return detect(group1->getEngineData(), group2->getEngineData(), + option, result); } //============================================================================== -CollisionDetector::CollisionDetector(std::unique_ptr&& engine) - : mEngine(std::move(engine)) +bool CollisionDetector::detect( + CollisionGroupData* group, CollisionObjectData* object, + const Option& option, Result& result) { - assert(mEngine); + return detect(object, group, option, result); } } // namespace collision diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 60e7d661d4f19..c88f188e1385e 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -58,17 +58,11 @@ class CollisionDetector : public std::enable_shared_from_this { public: - template - static CollisionDetectorPtr create(const Args&... args); + friend class CollisionObject; + friend class CollisionGroup; - /// Destructor - virtual ~CollisionDetector(); - - /// Return collision detection engine - Engine* getEngine(); - - /// Return (const) collision detection engine - const Engine* getEngine() const; + /// Return collision detection engine type in std::string + virtual const std::string& getType() const = 0; /// Create a collision object template @@ -78,10 +72,8 @@ class CollisionDetector : public std::enable_shared_from_this /// Create a collision group std::shared_ptr createCollisionGroup( - const std::vector& objects); - - /// Create a collision group - std::shared_ptr createCollisionGroup(); + const std::vector& objects + = std::vector()); /// Perform collision detection for object1-object2. bool detect(CollisionObject* object1, CollisionObject* object2, @@ -105,24 +97,43 @@ class CollisionDetector : public std::enable_shared_from_this protected: + using CollisionObjectPtrs = std::vector; + /// Constructor - CollisionDetector(std::unique_ptr&& engine); + CollisionDetector() = default; -protected: + /// Create collision detection engine specific data for CollisionObject + virtual std::unique_ptr createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) = 0; - std::unique_ptr mEngine; + /// Create collision detection engine specific data for CollisionGroup + virtual std::unique_ptr createCollisionGroupData( + CollisionGroup* parent, + const CollisionObjectPtrs& collObjects) = 0; -}; + /// Perform collision detection for object1-object2. + virtual bool detect(CollisionObjectData* object1, + CollisionObjectData* object2, + const Option& option, Result& result) = 0; -//============================================================================== -template -CollisionDetectorPtr CollisionDetector::create(const Args&... args) -{ - auto engine = new EngineType(args...); + /// Perform collision detection for object-group. + virtual bool detect(CollisionObjectData* object, CollisionGroupData* group, + const Option& option, Result& result) = 0; - return CollisionDetectorPtr(new CollisionDetector( - std::move(std::unique_ptr(engine)))); -} + /// Identical with detect(object, group, option, result) + bool detect(CollisionGroupData* group, CollisionObjectData* object, + const Option& option, Result& result); + + /// Perform collision detection for group. + virtual bool detect(CollisionGroupData* group, + const Option& option, Result& result) = 0; + + /// Perform collision detection for group1-group2. + virtual bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + const Option& option, Result& result) = 0; + +}; //============================================================================== template diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 00425450a6bd3..493977dc9a27c 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -51,7 +51,7 @@ CollisionGroup::CollisionGroup( const CollisionGroup::CollisionObjectPtrs& collObjects) : mCollisionDetector(collisionDetector), mCollisionObjects(collObjects), - mEngineData(mCollisionDetector->getEngine()->createCollisionGroupData( + mEngineData(mCollisionDetector->createCollisionGroupData( this, mCollisionObjects).release()) { assert(mCollisionDetector); @@ -116,7 +116,8 @@ bool CollisionGroup::hasCollisionObject( } //============================================================================== -void CollisionGroup::addCollisionObject(const CollisionObjectPtr& object) +void CollisionGroup::addCollisionObject( + const CollisionObjectPtr& object, bool init) { if (hasCollisionObject(object)) { @@ -124,33 +125,26 @@ void CollisionGroup::addCollisionObject(const CollisionObjectPtr& object) return; } + object->addGroup(this); + mCollisionObjects.push_back(object); - mEngineData->addCollisionObject(object); + mEngineData->addCollisionObject(object, init); } //============================================================================== void CollisionGroup::addCollisionObjects( - const CollisionGroup::CollisionObjectPtrs& objects) + const CollisionGroup::CollisionObjectPtrs& objects, bool init) { - bool added = false; - for (auto object : objects) - { - if (!hasCollisionObject(object)) - { - mCollisionObjects.push_back(object); - mEngineData->addCollisionObject(object, false); - added = true; - } - } + addCollisionObject(object, false); - if (added) + if (init) mEngineData->init(); } //============================================================================== void CollisionGroup::removeCollisionObject( - const CollisionGroup::CollisionObjectPtr& object) + const CollisionGroup::CollisionObjectPtr& object, bool init) { if (!object) return; @@ -161,24 +155,32 @@ void CollisionGroup::removeCollisionObject( if (mCollisionObjects.end() != result) { mCollisionObjects.erase(result); - mEngineData->removeCollisionObject(*result); + mEngineData->removeCollisionObject(*result, false); } + + if (init) + mEngineData->init(); } //============================================================================== void CollisionGroup::removeCollisionObjects( - const CollisionGroup::CollisionObjectPtrs& objects) + const CollisionGroup::CollisionObjectPtrs& objects, bool init) { for (auto object : objects) - removeCollisionObject(object); - // TODO(JS): there is a room for improving the perfomance + removeCollisionObject(object, false); + + if (init) + mEngineData->init(); } //============================================================================== -void CollisionGroup::removeAllCollisionObjects() +void CollisionGroup::removeAllCollisionObjects(bool init) { + // Notify the engine data first that the collision objects are going to be + // removed, then actually remove all the collision objects. + mEngineData->removeAllCollisionObjects(init); + mCollisionObjects.clear(); - mEngineData->removeAllCollisionObjects(); } //============================================================================== diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 5a3b9558d9cc8..ee63545045ce9 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -39,13 +39,13 @@ #include -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { -class Engine; +class CollisionDetector; class CollisionNode; /// Heterogeneous collision group @@ -84,19 +84,23 @@ class CollisionGroup bool hasCollisionObject(const CollisionObjectPtr& object) const; /// Add collision object to this CollisionGroup - void addCollisionObject(const CollisionObjectPtr& object); + void addCollisionObject(const CollisionObjectPtr& object, + bool init = true); /// Add collision objects to this CollisionGroup - void addCollisionObjects(const CollisionObjectPtrs& objects); + void addCollisionObjects(const CollisionObjectPtrs& objects, + bool init = true); /// Remove collision object from this CollisionGroup - void removeCollisionObject(const CollisionObjectPtr& object); + void removeCollisionObject(const CollisionObjectPtr& object, + bool init = true); /// Remove collision objects from this CollisionGroup - void removeCollisionObjects(const CollisionObjectPtrs& objects); + void removeCollisionObjects(const CollisionObjectPtrs& objects, + bool init = true); /// Remove all the collision object in this CollisionGroup - void removeAllCollisionObjects(); + void removeAllCollisionObjects(bool init = true); /// Return array of collision objects const CollisionObjectPtrs& getCollisionObjects(); @@ -132,7 +136,6 @@ class CollisionGroup /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void updateEngineData(); - // TODO(JS): remove protected: diff --git a/dart/collision/CollisionGroupData.cpp b/dart/collision/CollisionGroupData.cpp index 50dcc6bd36b27..873c99ce99b6b 100644 --- a/dart/collision/CollisionGroupData.cpp +++ b/dart/collision/CollisionGroupData.cpp @@ -42,24 +42,26 @@ namespace dart { namespace collision { //============================================================================== -CollisionGroupData::CollisionGroupData(Engine* engine, CollisionGroup* parent) - : mEngine(engine), +CollisionGroupData::CollisionGroupData( + CollisionDetector* collisionDetector, + CollisionGroup* parent) + : mCollisionDetector(collisionDetector), mParent(parent) { - assert(mEngine); + assert(mCollisionDetector); assert(mParent); } //============================================================================== -Engine* CollisionGroupData::getEngine() +CollisionDetector* CollisionGroupData::getCollisionDetector() { - return mEngine; + return mCollisionDetector; } //============================================================================== -const Engine* CollisionGroupData::getEngine() const +const CollisionDetector* CollisionGroupData::getCollisionDetector() const { - return mEngine; + return mCollisionDetector; } //============================================================================== diff --git a/dart/collision/CollisionGroupData.h b/dart/collision/CollisionGroupData.h index f4682286dfb8a..5416e301a82ab 100644 --- a/dart/collision/CollisionGroupData.h +++ b/dart/collision/CollisionGroupData.h @@ -51,15 +51,18 @@ class CollisionGroupData using CollisionObjectPtrs = std::vector; - CollisionGroupData(Engine* engine, CollisionGroup* parent); + CollisionGroupData(CollisionDetector* collisionDetector, + CollisionGroup* parent); + /// Initiate collision group data. This function should be called whenever + /// collision object is added or removed. virtual void init() = 0; virtual void addCollisionObject(const CollisionObjectPtr& object, bool init = true) = 0; -// virtual void addCollisionObjects(const CollisionObjectPtr& object) = 0; - // TODO(JS): notifyCollisionObjectUpdated()? + virtual void addCollisionObjects(const CollisionObjectPtrs& objects, + bool init = true) = 0; virtual void removeCollisionObject(const CollisionObjectPtr& object, bool init = true) = 0; @@ -74,9 +77,9 @@ class CollisionGroupData CollisionGroup* newCollisionGroup, const CollisionObjectPtrs& collObjects) const = 0; - Engine* getEngine(); + CollisionDetector* getCollisionDetector(); - const Engine* getEngine() const; + const CollisionDetector* getCollisionDetector() const; CollisionGroup* getCollisionGroup(); @@ -84,7 +87,7 @@ class CollisionGroupData protected: - Engine* mEngine; + CollisionDetector* mCollisionDetector; CollisionGroup* mParent; diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index e8c459cd0a5fa..33b6e8f9fcbba 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -87,13 +87,36 @@ CollisionObject::CollisionObject( const dynamics::ShapePtr& shape) : mCollisionDetector(collisionDetector), mShape(shape), - mEngineData(mCollisionDetector->getEngine()->createCollisionObjectData( - this, mShape).release()) + mEngineData( + mCollisionDetector->createCollisionObjectData(this, mShape).release()) { assert(mCollisionDetector); assert(mShape); } +//============================================================================== +void CollisionObject::addGroup(CollisionGroup* group) +{ + if (!group) + return; + + if (!hasGroup(group)) + mGroups.push_back(group); +} + +//============================================================================== +void CollisionObject::removeGroup(CollisionGroup* group) +{ + mGroups.erase(std::remove(mGroups.begin(), mGroups.end(), group), + mGroups.end()); +} + +//============================================================================== +bool CollisionObject::hasGroup(CollisionGroup* group) +{ + return std::find(mGroups.begin(), mGroups.end(), group) != mGroups.end(); +} + //============================================================================== std::shared_ptr FreeCollisionObject::create( const CollisionDetectorPtr& collisionDetector, diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index a5a49e2105649..da311b68a7153 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -39,7 +39,7 @@ #include -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" #include "dart/collision/SmartPointer.h" #include "dart/collision/CollisionObjectData.h" @@ -50,6 +50,8 @@ class CollisionObject { public: + friend class CollisionGroup; + /// Return collision detection engine associated with this CollisionObject CollisionDetector* getCollisionDetector() const; @@ -84,9 +86,15 @@ class CollisionObject protected: /// Contructor - CollisionObject(const CollisionDetectorPtr& engine, + CollisionObject(const CollisionDetectorPtr& collisionDetector, const dynamics::ShapePtr& shape); + void addGroup(CollisionGroup* group); + + void removeGroup(CollisionGroup* group); + + bool hasGroup(CollisionGroup* group); + protected: /// Collision detector @@ -98,6 +106,8 @@ class CollisionObject /// Collision detection engine specific data std::unique_ptr mEngineData; + std::vector mGroups; + }; /// FreeCollisionOjbect is a basic collision object whose transformation can be diff --git a/dart/collision/CollisionObjectData.cpp b/dart/collision/CollisionObjectData.cpp index 0940b2315ed9c..a26cc4f0bc51c 100644 --- a/dart/collision/CollisionObjectData.cpp +++ b/dart/collision/CollisionObjectData.cpp @@ -40,26 +40,25 @@ namespace dart { namespace collision { //============================================================================== -CollisionObjectData::CollisionObjectData( - Engine* engine, +CollisionObjectData::CollisionObjectData(CollisionDetector* collisionDetector, CollisionObject* parent) - : mEngine(engine), + : mCollisionDetector(collisionDetector), mParent(parent) { - assert(mEngine); + assert(mCollisionDetector); assert(mParent); } //============================================================================== -Engine* CollisionObjectData::getEngine() +CollisionDetector* CollisionObjectData::getCollisionDetector() { - return mEngine; + return mCollisionDetector; } //============================================================================== -const Engine* CollisionObjectData::getEngine() const +const CollisionDetector* CollisionObjectData::getCollisionDetector() const { - return mEngine; + return mCollisionDetector; } //============================================================================== diff --git a/dart/collision/CollisionObjectData.h b/dart/collision/CollisionObjectData.h index 5211fc200e174..4038d2e5a9b22 100644 --- a/dart/collision/CollisionObjectData.h +++ b/dart/collision/CollisionObjectData.h @@ -49,7 +49,8 @@ class CollisionObjectData { public: - CollisionObjectData(Engine* engine, CollisionObject* parent); + CollisionObjectData(CollisionDetector* collisionDetector, + CollisionObject* parent); virtual void updateTransform(const Eigen::Isometry3d& tf) = 0; @@ -60,9 +61,9 @@ class CollisionObjectData virtual void update() = 0; // TODO(JS): reorganize updateTansform/updateShape/update - Engine* getEngine(); + CollisionDetector* getCollisionDetector(); - const Engine* getEngine() const; + const CollisionDetector* getCollisionDetector() const; CollisionObject* getCollisionObject(); @@ -70,7 +71,7 @@ class CollisionObjectData protected: - Engine* mEngine; + CollisionDetector* mCollisionDetector; CollisionObject* mParent; diff --git a/dart/collision/Engine.cpp b/dart/collision/Engine.cpp deleted file mode 100644 index eb518b0d03112..0000000000000 --- a/dart/collision/Engine.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/Engine.h" - -namespace dart { -namespace collision { - -//============================================================================= -bool Engine::detect(CollisionGroupData* group, - CollisionObjectData* object, - const Option& option, Result& result) -{ - return detect(object, group, option, result); -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/Engine.h b/dart/collision/Engine.h deleted file mode 100644 index 283e844163418..0000000000000 --- a/dart/collision/Engine.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_ENGINE_H_ -#define DART_COLLISION_ENGINE_H_ - -#include - -#include "dart/collision/SmartPointer.h" -#include "dart/collision/Option.h" -#include "dart/collision/Result.h" -#include "dart/dynamics/SmartPointer.h" - -namespace dart { -namespace collision { - -class Engine -{ -public: - - using CollisionObjectPtrs = std::vector; - - /// Return collision detection engine type in std::string - virtual const std::string& getType() const = 0; - - /// Create collision detection engine specific data for CollisionObject - virtual std::unique_ptr createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) = 0; - - /// Create collision detection engine specific data for CollisionGroup - virtual std::unique_ptr createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) = 0; - - /// Perform collision detection for object1-object2. - virtual bool detect(CollisionObjectData* object1, - CollisionObjectData* object2, - const Option& option, Result& result) = 0; - - /// Perform collision detection for object-group. - virtual bool detect(CollisionObjectData* object, CollisionGroupData* group, - const Option& option, Result& result) = 0; - - /// Identical with detect(object, group, option, result) - bool detect(CollisionGroupData* group, CollisionObjectData* object, - const Option& option, Result& result); - - /// Perform collision detection for group. - virtual bool detect(CollisionGroupData* group, - const Option& option, Result& result) = 0; - - /// Perform collision detection for group1-group2. - virtual bool detect(CollisionGroupData* group1, CollisionGroupData* group2, - const Option& option, Result& result) = 0; - -}; - -} // namespace collision -} // namespace dart - -#include "dart/collision/detail/Engine.h" - -#endif // DART_COLLISION_ENGINE_H_ diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h index df6ec7fb6e72c..7f5f88dfd4a3c 100644 --- a/dart/collision/SmartPointer.h +++ b/dart/collision/SmartPointer.h @@ -44,13 +44,11 @@ namespace dart { namespace collision { DART_COMMON_MAKE_SHARED_WEAK(CollisionDetector) - -DART_COMMON_MAKE_SHARED_WEAK(Engine) -DART_COMMON_MAKE_SHARED_WEAK(FCLEngine) -DART_COMMON_MAKE_SHARED_WEAK(FCLMeshEngine) -DART_COMMON_MAKE_SHARED_WEAK(DARTEngine) +DART_COMMON_MAKE_SHARED_WEAK(FCLCollisionDetector) +DART_COMMON_MAKE_SHARED_WEAK(FCLMeshCollisionDetector) +DART_COMMON_MAKE_SHARED_WEAK(DARTCollisionDetector) #ifdef HAVE_BULLET_COLLISION - DART_COMMON_MAKE_SHARED_WEAK(BulletEngine) + DART_COMMON_MAKE_SHARED_WEAK(BulletCollisionDetector) #endif DART_COMMON_MAKE_SHARED_WEAK(CollisionObject) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 5860b84f1a44e..d2f5936a81e38 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -36,194 +36,263 @@ #include "dart/collision/bullet/BulletCollisionDetector.h" -#include +#include -#include "dart/collision/bullet/BulletCollisionNode.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Skeleton.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/bullet/BulletTypes.h" +#include "dart/collision/bullet/BulletCollisionObjectData.h" +#include "dart/collision/bullet/BulletCollisionGroupData.h" namespace dart { namespace collision { -//struct btContactResultCB : public btCollisionWorld::ContactResultCallback { -// virtual btScalar addSingleResult( -// btManifoldPoint& cp, -// const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, -// const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { -// Contact contactPair; -// contactPair.point = convertVector3(cp.getPositionWorldOnA()); -// contactPair.normal = convertVector3(cp.m_normalWorldOnB); -// contactPair.penetrationDepth = -cp.m_distance1; +namespace { -// mContacts.push_back(contactPair); - -// return 0; -// } - -// std::vector mContacts; -//}; - -//============================================================================== -struct CollisionFilter : public btOverlapFilterCallback +struct BulletContactResultCallback : btCollisionWorld::ContactResultCallback { - // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* _proxy0, - btBroadphaseProxy* _proxy1) const + BulletContactResultCallback(Result& result) + : ContactResultCallback(), + mResult(result) { - assert((_proxy0 != nullptr && _proxy1 != nullptr) && - "Bullet broadphase overlapping pair proxies are nullptr"); + } - bool collide = (_proxy0->m_collisionFilterGroup & - _proxy1->m_collisionFilterMask) != 0; - collide = collide && (_proxy1->m_collisionFilterGroup & - _proxy0->m_collisionFilterMask); + btScalar addSingleResult(btManifoldPoint& cp, + const btCollisionObjectWrapper* colObj0Wrap, + int partId0, + int index0, + const btCollisionObjectWrapper* colObj1Wrap, + int partId1, + int index1) override; - if (collide) - { - btCollisionObject* collObj0 = - static_cast(_proxy0->m_clientObject); - btCollisionObject* collObj1 = - static_cast(_proxy1->m_clientObject); + Result& mResult; +}; - BulletUserData* userData0 = - static_cast(collObj0->getUserPointer()); - BulletUserData* userData1 = - static_cast(collObj1->getUserPointer()); +} // anonymous namespace - // Assume single collision detector - assert(userData0->btCollDet == userData1->btCollDet); - CollisionDetector* cd = userData0->btCollDet; - CollisionNode* cn1 = userData0->btCollNode; - CollisionNode* cn2 = userData1->btCollNode; +//============================================================================== +std::shared_ptr BulletCollisionDetector::create() +{ + return std::shared_ptr( + new BulletCollisionDetector()); +} - collide = cd->isCollidable(cn1, cn2); - } +//============================================================================== +const std::string& BulletCollisionDetector::getTypeStatic() +{ + static const std::string& type("Bullet"); + return type; +} - return collide; - } -}; +//============================================================================== +const std::string& BulletCollisionDetector::getType() const +{ + return getTypeStatic(); +} //============================================================================== -BulletCollisionDetector::BulletCollisionDetector() : CollisionDetector() +std::unique_ptr +BulletCollisionDetector::createCollisionObjectData(CollisionObject* parent, + const dynamics::ShapePtr& shape) { -// btVector3 worldAabbMin(-1000, -1000, -1000); -// btVector3 worldAabbMax(1000, 1000, 1000); -// btBroadphaseInterface* broadphasePairCache = -// new btAxisSweep3(worldAabbMin, worldAabbMax); - btBroadphaseInterface* broadphasePairCache = new btDbvtBroadphase(); - btCollisionConfiguration* collisionConfiguration = - new btDefaultCollisionConfiguration(); - btDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); - - mBulletCollisionWorld = new btCollisionWorld(dispatcher, - broadphasePairCache, - collisionConfiguration); - - btOverlapFilterCallback* filterCallback = new CollisionFilter(); - btOverlappingPairCache* pairCache = mBulletCollisionWorld->getPairCache(); - assert(pairCache != nullptr); - pairCache->setOverlapFilterCallback(filterCallback); + return std::unique_ptr( + new BulletCollisionObjectData(this, parent, shape)); } //============================================================================== -BulletCollisionDetector::~BulletCollisionDetector() +std::unique_ptr +BulletCollisionDetector::createCollisionGroupData( + CollisionGroup* parent, + const CollisionObjectPtrs& collObjects) { + return std::unique_ptr( + new BulletCollisionGroupData(this, parent, collObjects)); } //============================================================================== -CollisionNode* BulletCollisionDetector::createCollisionNode( - dynamics::BodyNode* _bodyNode) +Contact convertContact(const btManifoldPoint& bulletManifoldPoint, + const BulletCollisionObjectUserData* userData1, + const BulletCollisionObjectUserData* userData2) { - BulletCollisionNode* collNode = new BulletCollisionNode(_bodyNode); + assert(userData1); + assert(userData2); - for (int i = 0; i < collNode->getNumBulletCollisionObjects(); ++i) - { - btCollisionObject* collObj = collNode->getBulletCollisionObject(i); - BulletUserData* userData - = static_cast(collObj->getUserPointer()); - userData->btCollDet = this; - mBulletCollisionWorld->addCollisionObject(collObj); - } + Contact contact; + + contact.point = convertVector3(bulletManifoldPoint.getPositionWorldOnA()); + contact.normal = convertVector3(bulletManifoldPoint.m_normalWorldOnB); + contact.penetrationDepth = -bulletManifoldPoint.m_distance1; + contact.collisionObject1 = userData1->collisionObject; + contact.collisionObject2 = userData2->collisionObject; - return collNode; + return contact; } //============================================================================== -bool BulletCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, - bool /*_calculateContactPoints*/) +void convertContacts(btCollisionWorld* collWorld, Result& result) { - // Update all the transformations of the collision nodes - for (size_t i = 0; i < mCollisionNodes.size(); ++i) - static_cast( - mCollisionNodes[i])->updateBulletCollisionObjects(); - - // Setting up broadphase collision detection options - btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); - dispatchInfo.m_timeStep = 0.001; - dispatchInfo.m_stepCount = 0; - // dispatchInfo.m_debugDraw = getDebugDrawer(); - - // Collision detection - mBulletCollisionWorld->performDiscreteCollisionDetection(); -// std::cout << "Number of collision objects: " -// << collWorld->getNumCollisionObjects() << std::endl; - - // Clear mContacts which is the list of old contacts - clearAllContacts(); - - // Set all the body nodes are not in colliding - for (size_t i = 0; i < mCollisionNodes.size(); i++) - mCollisionNodes[i]->getBodyNode()->setColliding(false); - - // Add all the contacts to mContacts - int numManifolds = mBulletCollisionWorld->getDispatcher()->getNumManifolds(); - btDispatcher* dispatcher = mBulletCollisionWorld->getDispatcher(); - for (int i = 0; i < numManifolds; ++i) + assert(collWorld); + + auto dispatcher = collWorld->getDispatcher(); + assert(dispatcher); + + auto numManifolds = dispatcher->getNumManifolds(); + + for (auto i = 0; i < numManifolds; ++i) { - btPersistentManifold* contactManifold - = dispatcher->getManifoldByIndexInternal(i); - const btCollisionObject* obA = contactManifold->getBody0(); - const btCollisionObject* obB = contactManifold->getBody1(); - - BulletUserData* userDataA - = static_cast(obA->getUserPointer()); - BulletUserData* userDataB - = static_cast(obB->getUserPointer()); - - int numContacts = contactManifold->getNumContacts(); - for (int j = 0; j < numContacts; j++) - { - btManifoldPoint& cp = contactManifold->getContactPoint(j); + auto contactManifold = dispatcher->getManifoldByIndexInternal(i); + const auto bulletCollObj0 = contactManifold->getBody0(); + const auto bulletCollObj1 = contactManifold->getBody1(); + + auto userPointer0 = bulletCollObj0->getUserPointer(); + auto userPointer1 = bulletCollObj1->getUserPointer(); + + auto userDataA = static_cast(userPointer1); + auto userDataB = static_cast(userPointer0); - Contact contactPair; - contactPair.point = convertVector3(cp.getPositionWorldOnA()); - contactPair.normal = convertVector3(cp.m_normalWorldOnB); - contactPair.penetrationDepth = -cp.m_distance1; - contactPair.bodyNode1 = userDataA->btCollNode->getBodyNode(); - contactPair.bodyNode2 = userDataB->btCollNode->getBodyNode(); + auto numContacts = contactManifold->getNumContacts(); - mContacts.push_back(contactPair); + for (auto j = 0; j < numContacts; ++j) + { + auto& cp = contactManifold->getContactPoint(j); - // Set these two bodies are in colliding - contactPair.bodyNode1.lock()->setColliding(true); - contactPair.bodyNode2.lock()->setColliding(true); + result.contacts.push_back(convertContact(cp, userDataA, userDataB)); } } +} + +//============================================================================== +bool BulletCollisionDetector::detect( + CollisionObjectData* objectData1, + CollisionObjectData* objectData2, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(objectData1->getCollisionDetector()->getType() + == BulletCollisionDetector::getTypeStatic()); + assert(objectData2->getCollisionDetector()->getType() + == BulletCollisionDetector::getTypeStatic()); + + auto castedData1 = static_cast(objectData1); + auto castedData2 = static_cast(objectData2); + + auto bulletCollObj1 = castedData1->getBulletCollisionObject(); + auto bulletCollObj2 = castedData2->getBulletCollisionObject(); + + if (!mBulletCollisionGroupForSinglePair) + mBulletCollisionGroupForSinglePair = createCollisionGroup(); - // Return true if there are contacts - return !mContacts.empty(); + auto cb = BulletContactResultCallback(result); + auto collisionGroupData = static_cast( + mBulletCollisionGroupForSinglePair->getEngineData()); + auto bulletCollisionWorld = collisionGroupData->getBulletCollisionWorld(); + bulletCollisionWorld->contactPairTest(bulletCollObj1, bulletCollObj2, cb); + + return !result.contacts.empty(); +} + +//============================================================================== +bool BulletCollisionDetector::detect( + CollisionObjectData* objectData, + CollisionGroupData* groupData, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(objectData); + assert(groupData); + assert(objectData->getCollisionDetector()->getType() + == BulletCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() + == BulletCollisionDetector::getTypeStatic()); + + auto castedObjData = static_cast(objectData); + auto castedGrpData = static_cast(groupData); + + auto bulletCollObj = castedObjData->getBulletCollisionObject(); + auto bulletCollisionWorld = castedGrpData->getBulletCollisionWorld(); + + auto cb = BulletContactResultCallback(result); + bulletCollisionWorld->contactTest(bulletCollObj, cb); + + return !result.contacts.empty(); +} + +//============================================================================== +bool BulletCollisionDetector::detect( + CollisionGroupData* groupData, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(groupData); + assert(groupData->getCollisionDetector()->getType() + == BulletCollisionDetector::getTypeStatic()); + + auto castedData = static_cast(groupData); + + auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); + + bulletCollisionWorld->performDiscreteCollisionDetection(); + + convertContacts(bulletCollisionWorld, result); + + return !result.contacts.empty(); } //============================================================================== -bool BulletCollisionDetector::detectCollision(CollisionNode* /*_node1*/, - CollisionNode* /*_node2*/, - bool /*_calculateContactPoints*/) +bool BulletCollisionDetector::detect( + CollisionGroupData* groupData1, + CollisionGroupData* groupData2, + const Option& /*option*/, Result& result) +{ + result.contacts.clear(); + + assert(groupData1); + assert(groupData2); + assert(groupData1->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); + assert(groupData2->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); + + auto castedData1 = static_cast(groupData1); + auto castedData2 = static_cast(groupData2); + + auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); + auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); + +// BulletCollisionData collData(&option, &result); +// bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); + + return !result.contacts.empty(); +} + + + + +namespace { + +btScalar BulletContactResultCallback::addSingleResult( + btManifoldPoint& cp, + const btCollisionObjectWrapper* colObj0Wrap, + int /*partId0*/, + int /*index0*/, + const btCollisionObjectWrapper* colObj1Wrap, + int /*partId1*/, + int /*index1*/) { - assert(false); - return false; + auto userPointer0 = colObj0Wrap->getCollisionObject()->getUserPointer(); + auto userPointer1 = colObj1Wrap->getCollisionObject()->getUserPointer(); + + auto userDataA = static_cast(userPointer1); + auto userDataB = static_cast(userPointer0); + + mResult.contacts.push_back(convertContact(cp, userDataA, userDataB)); + + return 1.0f; } -} // namespace collision -} // namespace dart +} // anonymous namespace + +} // namespace collision +} // namespace dart diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index a88f5d7ddb07f..b5c69adff1de5 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -38,44 +38,65 @@ #define DART_COLLISION_BULLET_BULLETCOLLISIONDETECTOR_H_ #include -#include - #include -#include - #include "dart/collision/CollisionDetector.h" -#include "dart/collision/bullet/BulletTypes.h" namespace dart { namespace collision { -class BulletCollisionNode; +class CollisionGroup; -/// @brief class BulletCollisionDetector class BulletCollisionDetector : public CollisionDetector { public: - /// @brief Constructor - BulletCollisionDetector(); - /// @brief Destructor - virtual ~BulletCollisionDetector(); + friend class CollisionDetector; + + static std::shared_ptr create(); + + /// Return engine type "Bullet" + static const std::string& getTypeStatic(); + + // Documentation inherit + const std::string& getType() const override; + + using CollisionDetector::detect; + +protected: + + /// Constructor + BulletCollisionDetector() = default; + + // Documentation inherit + std::unique_ptr createCollisionObjectData( + CollisionObject* parent, + const dynamics::ShapePtr& shape) override; + + // Documentation inherit + std::unique_ptr createCollisionGroupData( + CollisionGroup* parent, + const CollisionObjectPtrs& collObjects) override; - /// \copydoc CollisionDetector::createCollisionNode - virtual CollisionNode* createCollisionNode(dynamics::BodyNode* _bodyNode); + // Documentation inherit + bool detect(CollisionObjectData* object1, CollisionObjectData* object2, + const Option& option, Result& result) override; - /// \copydoc CollisionDetector::detectCollision - virtual bool detectCollision(bool _checkAllCollisions, - bool _calculateContactPoints); + // Documentation inherit + bool detect(CollisionObjectData* object, CollisionGroupData* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroupData* group, + const Option& option, Result& result) override; + + // Documentation inherit + bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + const Option& option, Result& result) override; protected: - // TODO(JS): Not implemented yet. - /// \copydoc CollisionDetector::detectCollision - virtual bool detectCollision(CollisionNode* _node1, CollisionNode* _node2, - bool _calculateContactPoints); - /// @brief Bullet collision world - btCollisionWorld* mBulletCollisionWorld; + std::shared_ptr mBulletCollisionGroupForSinglePair; + }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionGroupData.cpp b/dart/collision/bullet/BulletCollisionGroupData.cpp new file mode 100644 index 0000000000000..90507cb0ab477 --- /dev/null +++ b/dart/collision/bullet/BulletCollisionGroupData.cpp @@ -0,0 +1,190 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/bullet/BulletCollisionGroupData.h" + +#include "dart/collision/CollisionObject.h" +#include "dart/collision/bullet/BulletCollisionObjectData.h" + +namespace dart { +namespace collision { + +//============================================================================== +BulletCollisionGroupData::BulletCollisionGroupData( + CollisionDetector* collisionDetector, + CollisionGroup* parent, + const BulletCollisionGroupData::CollisionObjects& collObjects) + : CollisionGroupData(collisionDetector, parent), + mBulletProadphaseAlg( + new btDbvtBroadphase()), + mBulletCollisionConfiguration( + new btDefaultCollisionConfiguration()), + mBulletDispatcher( + new btCollisionDispatcher(mBulletCollisionConfiguration.get())), + mBulletCollisionWorld( + new btCollisionWorld(mBulletDispatcher.get(), + mBulletProadphaseAlg.get(), + mBulletCollisionConfiguration.get())) +{ + btOverlapFilterCallback* filterCallback = new CollisionFilter(); + btOverlappingPairCache* pairCache = mBulletCollisionWorld->getPairCache(); + assert(pairCache != nullptr); + pairCache->setOverlapFilterCallback(filterCallback); + + addCollisionObjects(collObjects, true); +} + +//============================================================================== +std::unique_ptr BulletCollisionGroupData::clone( + CollisionGroup* newParent, + const CollisionObjectPtrs& collObjects) const +{ + return std::unique_ptr( + new BulletCollisionGroupData(mCollisionDetector, newParent, collObjects)); +} + +//============================================================================== +void BulletCollisionGroupData::init() +{ + btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); + // dispatchInfo.m_timeStep = 0.001; + dispatchInfo.m_stepCount = 0; +} + +//============================================================================== +void BulletCollisionGroupData::addCollisionObject( + const CollisionObjectPtr& object, const bool init) +{ + auto data = static_cast(object->getEngineData()); + mBulletCollisionWorld->addCollisionObject(data->getBulletCollisionObject()); + + if (init) + this->init(); +} + +//============================================================================== +void BulletCollisionGroupData::addCollisionObjects( + const BulletCollisionGroupData::CollisionObjectPtrs& collObjects, + const bool init) +{ + for (auto collObj : collObjects) + { + auto data = static_cast( + collObj->getEngineData()); + + mBulletCollisionWorld->addCollisionObject(data->getBulletCollisionObject()); + } + + if (init) + this->init(); +} + +//============================================================================== +void BulletCollisionGroupData::removeCollisionObject( + const CollisionObjectPtr& object, const bool init) +{ + auto data = static_cast(object->getEngineData()); + mBulletCollisionWorld->removeCollisionObject( + data->getBulletCollisionObject()); + + if (init) + this->init(); +} + +//============================================================================== +void BulletCollisionGroupData::removeAllCollisionObjects(bool init) +{ + auto collisionObjects = mParent->getCollisionObjects(); + for (auto collisionObject : collisionObjects) + removeCollisionObject(collisionObject, false); + + if (init) + this->init(); +} + +//============================================================================== +void BulletCollisionGroupData::update() +{ + // Setting up broadphase collision detection options + btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); + // dispatchInfo.m_timeStep = 0.001; + dispatchInfo.m_stepCount = 0; + + mBulletCollisionWorld->updateAabbs(); +} + +//============================================================================== +btCollisionWorld* BulletCollisionGroupData::getBulletCollisionWorld() const +{ + return mBulletCollisionWorld.get(); +} + +//============================================================================== +bool BulletCollisionGroupData::CollisionFilter::needBroadphaseCollision( + btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const +{ + assert((proxy0 != nullptr && proxy1 != nullptr) && + "Bullet broadphase overlapping pair proxies are nullptr"); + + bool collide = (proxy0->m_collisionFilterGroup & + proxy1->m_collisionFilterMask) != 0; + collide = collide && (proxy1->m_collisionFilterGroup & + proxy0->m_collisionFilterMask); + + if (collide) + { + auto collObj0 = static_cast(proxy0->m_clientObject); + auto collObj1 = static_cast(proxy1->m_clientObject); + + auto userData0 = static_cast(collObj0->getUserPointer()); + auto userData1 = static_cast(collObj1->getUserPointer()); + + // Assume single collision detector + assert(userData0->collisionDetector == userData1->collisionDetector); + + auto collGroup0 = userData0->group; + auto collGroup1 = userData1->group; + +// if (!collGroup0 || !) + + // TODO(JS): check collision pair if they are enabled. + } + + return collide; +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/bullet/BulletCollisionGroupData.h b/dart/collision/bullet/BulletCollisionGroupData.h new file mode 100644 index 0000000000000..80acd79b3735b --- /dev/null +++ b/dart/collision/bullet/BulletCollisionGroupData.h @@ -0,0 +1,120 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONGROUPDATA_H_ +#define DART_COLLISION_BULLET_BULLETCOLLISIONGROUPDATA_H_ + +#include + +#include "dart/collision/CollisionGroup.h" +#include "dart/collision/CollisionGroupData.h" + +namespace dart { +namespace collision { + +class CollisionObject; +class BulletCollisionObjectUserData; + +class BulletCollisionGroupData : public CollisionGroupData +{ +public: + + using CollisionObjects = CollisionGroup::CollisionObjectPtrs; + + /// Constructor + BulletCollisionGroupData( + CollisionDetector* collisionDetector, + CollisionGroup* parent, + const CollisionObjects& collObjects = CollisionObjects()); + + // Documentation inherited + std::unique_ptr clone( + CollisionGroup* newParent, + const CollisionObjectPtrs& collObjects) const override; + + // Documentation inherited + void init() override; + + // Documentation inherited + void addCollisionObject(const CollisionObjectPtr& object, + const bool init) override; + + // Documentation inherited + void addCollisionObjects(const CollisionObjectPtrs& collObjects, + const bool init) override; + + // Documentation inherited + void removeCollisionObject(const CollisionObjectPtr& object, + const bool init) override; + + // Documentation inherited + void removeAllCollisionObjects(bool init) override; + + // Documentation inherited + void update() override; + + /// Return Bullet collision world + btCollisionWorld* getBulletCollisionWorld() const; + +protected: + + struct CollisionFilter : public btOverlapFilterCallback + { + // return true when pairs need collision + bool needBroadphaseCollision(btBroadphaseProxy* proxy0, + btBroadphaseProxy* proxy1) const override; + }; + + /// Bullet broad-phase algorithm + std::unique_ptr mBulletProadphaseAlg; + + /// Bullet collision filter + std::unique_ptr mBulletCollisionFilter; + + /// Bullet collision configuration + std::unique_ptr mBulletCollisionConfiguration; + + /// Bullet collision dispatcher + std::unique_ptr mBulletDispatcher; + + /// Bullet collision world + std::unique_ptr mBulletCollisionWorld; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_BULLET_BULLETCOLLISIONGROUPDATA_H_ diff --git a/dart/collision/bullet/BulletCollisionNode.cpp b/dart/collision/bullet/BulletCollisionNode.cpp deleted file mode 100644 index 23872260b1e1f..0000000000000 --- a/dart/collision/bullet/BulletCollisionNode.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/* - * Copyright (c) 2014-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/bullet/BulletCollisionNode.h" - -#include - -#include - -#include "dart/collision/bullet/BulletTypes.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/MeshShape.h" - -namespace dart { -namespace collision { - -//============================================================================== -BulletCollisionNode::BulletCollisionNode(dynamics::BodyNode* _bodyNode) - : CollisionNode(_bodyNode) -{ - for (size_t i = 0; i < _bodyNode->getNumCollisionShapes(); i++) - { - dynamics::ConstShapePtr shape = _bodyNode->getCollisionShape(i); - switch (shape->getShapeType()) - { - case dynamics::Shape::BOX: - { - const dynamics::BoxShape* box = - static_cast(shape.get()); - - btBoxShape* btBox = new btBoxShape(btVector3(box->getSize()[0]*0.5, - box->getSize()[1]*0.5, - box->getSize()[2]*0.5)); - btCollisionObject* btCollObj = new btCollisionObject(); - btCollObj->setCollisionShape(btBox); - BulletUserData* userData = new BulletUserData; - userData->bodyNode = _bodyNode; - userData->shape = shape; - userData->btCollNode = this; - btCollObj->setUserPointer(userData); - mbtCollsionObjects.push_back(btCollObj); - - break; - } - case dynamics::Shape::ELLIPSOID: - { - const dynamics::EllipsoidShape* ellipsoid = - static_cast(shape.get()); - - if (ellipsoid->isSphere()) - { - btSphereShape* btSphere = new btSphereShape(ellipsoid->getSize()[0] * - 0.5); - btCollisionObject* btCollObj = new btCollisionObject(); - btCollObj->setCollisionShape(btSphere); - BulletUserData* userData = new BulletUserData; - userData->bodyNode = _bodyNode; - userData->shape = shape; - userData->btCollNode = this; - btCollObj->setUserPointer(userData); - mbtCollsionObjects.push_back(btCollObj); - } - else - { - // TODO(JS): Add mesh for ellipsoid - } - - break; - } - case dynamics::Shape::CYLINDER: - { - const dynamics::CylinderShape* cylinder = - static_cast(shape.get()); - - btCylinderShapeZ* btCylinder = - new btCylinderShapeZ(btVector3(cylinder->getRadius(), - cylinder->getRadius(), - cylinder->getHeight() * 0.5)); - btCollisionObject* btCollObj = new btCollisionObject(); - btCollObj->setCollisionShape(btCylinder); - BulletUserData* userData = new BulletUserData; - userData->bodyNode = _bodyNode; - userData->shape = shape; - userData->btCollNode = this; - btCollObj->setUserPointer(userData); - mbtCollsionObjects.push_back(btCollObj); - - break; - } - case dynamics::Shape::PLANE: - { - const dynamics::PlaneShape* plane = - static_cast(shape.get()); - - btScalar d = plane->getOffset(); - - btStaticPlaneShape* btStaticPlane = - new btStaticPlaneShape(convertVector3(plane->getNormal()), d); - btCollisionObject* btCollObj = new btCollisionObject(); - btCollObj->setCollisionShape(btStaticPlane); - BulletUserData* userData = new BulletUserData; - userData->bodyNode = _bodyNode; - userData->shape = shape; - userData->btCollNode = this; - btCollObj->setUserPointer(userData); - mbtCollsionObjects.push_back(btCollObj); - - break; - } - case dynamics::Shape::MESH: - { - const dynamics::MeshShape* shapeMesh - = static_cast(shape.get()); - btConvexTriangleMeshShape* btMesh = _createMesh(shapeMesh->getScale(), - shapeMesh->getMesh()); - btCollisionObject* btCollObj = new btCollisionObject(); - - // Add user data - btCollObj->setCollisionShape(btMesh); - BulletUserData* userData = new BulletUserData; - userData->bodyNode = _bodyNode; - userData->shape = shape; - userData->btCollNode = this; - btCollObj->setUserPointer(userData); - - // - mbtCollsionObjects.push_back(btCollObj); - - break; - } - default: - { - std::cout << "ERROR: Collision checking does not support " - << _bodyNode->getName() - << "'s Shape type\n"; - break; - } - } - } -} - -//============================================================================== -BulletCollisionNode::~BulletCollisionNode() -{ -} - -//============================================================================== -void BulletCollisionNode::updateBulletCollisionObjects() -{ - for (size_t i = 0; i < mbtCollsionObjects.size(); ++i) - { - BulletUserData* userData = - static_cast(mbtCollsionObjects[i]->getUserPointer()); - dynamics::ConstShapePtr shape = userData->shape; - btTransform T = convertTransform(mBodyNode->getTransform() * - shape->getLocalTransform()); - mbtCollsionObjects[i]->setWorldTransform(T); - } -} - -//============================================================================== -int BulletCollisionNode::getNumBulletCollisionObjects() const -{ - return mbtCollsionObjects.size(); -} - -//============================================================================== -btCollisionObject*BulletCollisionNode::getBulletCollisionObject(int _i) -{ - return mbtCollsionObjects[_i]; -} - -//============================================================================== -btConvexTriangleMeshShape* _createMesh(const Eigen::Vector3d& _scale, - const aiScene* _mesh) -{ - btTriangleMesh* btMesh = new btTriangleMesh(); - - for (unsigned int i = 0; i < _mesh->mNumMeshes; i++) - { - for (unsigned int j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) - { - btVector3 vertices[3]; - for (unsigned int k = 0; k < 3; k++) - { - const aiVector3D& vertex = _mesh->mMeshes[i]->mVertices[ - _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = btVector3(vertex.x * _scale[0], - vertex.y * _scale[1], - vertex.z * _scale[2]); - } - btMesh->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - - btConvexTriangleMeshShape* btMeshShape = - new btConvexTriangleMeshShape(btMesh); - - return btMeshShape; -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/bullet/BulletCollisionNode.h b/dart/collision/bullet/BulletCollisionNode.h deleted file mode 100644 index 45102756fbe7f..0000000000000 --- a/dart/collision/bullet/BulletCollisionNode.h +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Copyright (c) 2014-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONNODE_H_ -#define DART_COLLISION_BULLET_BULLETCOLLISIONNODE_H_ - -#include - -#include -#include -#include - -#include "dart/dynamics/SmartPointer.h" -#include "dart/dynamics/Shape.h" -#include "dart/collision/CollisionNode.h" - -namespace dart { -namespace dynamics { -class BodyNode; -} // namespace dynamics -} // namespace dart - -namespace dart { -namespace collision { - -class BulletCollisionNode; -class BulletCollisionDetector; - -struct BulletUserData { - dynamics::BodyNode* bodyNode; - dynamics::ConstShapePtr shape; - BulletCollisionNode* btCollNode; - BulletCollisionDetector* btCollDet; -}; - -/// @brief class BulletCollisionNode -class BulletCollisionNode : public CollisionNode -{ -public: - /// @brief Constructor - explicit BulletCollisionNode(dynamics::BodyNode* _bodyNode); - - /// @brief Destructor - virtual ~BulletCollisionNode(); - - /// @brief Update transformation of all the bullet collision objects. - void updateBulletCollisionObjects(); - - /// @brief Get number of bullet collision objects - int getNumBulletCollisionObjects() const; - - /// @brief Get bullet collision object whose index is _i - btCollisionObject* getBulletCollisionObject(int _i); - -private: - /// @brief Bullet collision objects - std::vector mbtCollsionObjects; -}; - -/// @brief Create Bullet mesh from assimp3 mesh -btConvexTriangleMeshShape* _createMesh(const Eigen::Vector3d& _scale, - const aiScene* _mesh); - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_BULLET_BULLETCOLLISIONNODE_H_ diff --git a/dart/collision/bullet/BulletCollisionObjectData.cpp b/dart/collision/bullet/BulletCollisionObjectData.cpp new file mode 100644 index 0000000000000..68938aa963117 --- /dev/null +++ b/dart/collision/bullet/BulletCollisionObjectData.cpp @@ -0,0 +1,268 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/bullet/BulletCollisionObjectData.h" + +#include "dart/common/Console.h" +#include "dart/collision/bullet/BulletTypes.h" +#include "dart/collision/CollisionDetector.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/bullet/BulletTypes.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" + +namespace dart { +namespace collision { + +//============================================================================== +BulletCollisionObjectUserData::BulletCollisionObjectUserData() + : shape(nullptr), + collisionObject(nullptr), + collisionDetector(nullptr), + group(nullptr) +{ + // Do nothing +} + +//============================================================================== +BulletCollisionObjectData::BulletCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent, + const dynamics::ShapePtr& shape) + : CollisionObjectData(collisionDetector, parent) +{ + updateShape(shape); +} + +//============================================================================== +void BulletCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) +{ + mBulletCollisionObject->setWorldTransform(convertTransform(tf)); +} + +//============================================================================== +void BulletCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) +{ + mBulletTriangleMesh.release(); + mBulletGImpactMeshShape.release(); + mBulletCollisionShape.release(); + mBulletCollisionObject.release(); + + using dynamics::Shape; + using dynamics::BoxShape; + using dynamics::EllipsoidShape; + using dynamics::CylinderShape; + using dynamics::PlaneShape; + using dynamics::MeshShape; + using dynamics::SoftMeshShape; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + + const auto box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); + + mBulletCollisionShape.reset(new btBoxShape(convertVector3(size*0.5))); + + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + + const auto ellipsoid = static_cast(shape.get()); + const Eigen::Vector3d& size = ellipsoid->getSize(); + + if (ellipsoid->isSphere()) + { + mBulletCollisionShape.reset(new btSphereShape(size[0] * 0.5)); + } + else + { + // TODO(JS): Add mesh for ellipsoid + } + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + + const auto cylinder = static_cast(shape.get()); + const auto radius = cylinder->getRadius(); + const auto height = cylinder->getHeight(); + const auto size = btVector3(radius, radius, height * 0.5); + + mBulletCollisionShape.reset(new btCylinderShapeZ(size)); + + break; + } + case Shape::PLANE: + { + assert(dynamic_cast(shape.get())); + + const auto plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + + mBulletCollisionShape.reset(new btStaticPlaneShape( + convertVector3(normal), offset)); + + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + + const auto shapeMesh = static_cast(shape.get()); + const auto scale = shapeMesh->getScale(); + const auto mesh = shapeMesh->getMesh(); + + mBulletCollisionShape.reset(createMesh(scale, mesh)); + + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + + const auto softMeshShape = static_cast(shape.get()); + const auto mesh = softMeshShape->getAssimpMesh(); + + mBulletCollisionShape.reset(createSoftMesh(mesh)); + + break; + } + default: + { + dterr << "[BulletCollisionObjectData::init] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + + mBulletCollisionShape.reset(new btSphereShape(0.1)); + + break; + } + } + + mBulletCollisionObjectUserData.reset(new BulletCollisionObjectUserData()); + mBulletCollisionObjectUserData->shape = shape; + mBulletCollisionObjectUserData->collisionObject = mParent; + mBulletCollisionObjectUserData->collisionDetector = mCollisionDetector; + mBulletCollisionObjectUserData->group = nullptr; + + mBulletCollisionObject.reset(new btCollisionObject()); + mBulletCollisionObject->setCollisionShape(mBulletCollisionShape.get()); + mBulletCollisionObject->setUserPointer(mBulletCollisionObjectUserData.get()); +} + +//============================================================================== +void BulletCollisionObjectData::update() +{ + updateTransform(mParent->getTransform()); +} + +//============================================================================== +btCollisionObject* BulletCollisionObjectData::getBulletCollisionObject() const +{ + return mBulletCollisionObject.get(); +} + +//============================================================================== +btGImpactMeshShape* BulletCollisionObjectData::createMesh( + const Eigen::Vector3d& scale, const aiScene* mesh) +{ + mBulletTriangleMesh.reset(new btTriangleMesh()); + + for (unsigned int i = 0; i < mesh->mNumMeshes; i++) + { + for (unsigned int j = 0; j < mesh->mMeshes[i]->mNumFaces; j++) + { + btVector3 vertices[3]; + for (unsigned int k = 0; k < 3; k++) + { + const aiVector3D& vertex = mesh->mMeshes[i]->mVertices[ + mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + vertices[k] = btVector3(vertex.x * scale[0], + vertex.y * scale[1], + vertex.z * scale[2]); + } + mBulletTriangleMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + } + } + + btGImpactMeshShape* gimpactMeshShape + = new btGImpactMeshShape(mBulletTriangleMesh.get()); + gimpactMeshShape->updateBound(); + + return gimpactMeshShape; +} + +//============================================================================== +btGImpactMeshShape* BulletCollisionObjectData::createSoftMesh( + const aiMesh* mesh) +{ + mBulletTriangleMesh.reset(new btTriangleMesh()); + + for (auto i = 0u; i < mesh->mNumFaces; ++i) + { + btVector3 vertices[3]; + for (auto j = 0u; j < 3; ++j) + { + const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; + vertices[j] = btVector3(vertex.x, vertex.y, vertex.z); + } + mBulletTriangleMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + btGImpactMeshShape* gimpactMeshShape + = new btGImpactMeshShape(mBulletTriangleMesh.get()); + gimpactMeshShape->updateBound(); + + return gimpactMeshShape; +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/bullet/BulletCollisionObjectData.h b/dart/collision/bullet/BulletCollisionObjectData.h new file mode 100644 index 0000000000000..6d4e48515506b --- /dev/null +++ b/dart/collision/bullet/BulletCollisionObjectData.h @@ -0,0 +1,112 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONOBJECTDATA_H_ +#define DART_COLLISION_BULLET_BULLETCOLLISIONOBJECTDATA_H_ + +#include +#include +#include +#include +#include +#include +#include "dart/collision/CollisionObjectData.h" + +namespace dart { +namespace collision { + +class CollisionObject; + +struct BulletCollisionObjectUserData +{ + dynamics::ConstShapePtr shape; + CollisionObject* collisionObject; + + CollisionDetector* collisionDetector; + CollisionGroup* group; + + BulletCollisionObjectUserData(); +}; + +class BulletCollisionObjectData : public CollisionObjectData +{ +public: + + /// Constructor + BulletCollisionObjectData(CollisionDetector* collisionDetector, + CollisionObject* parent, + const dynamics::ShapePtr& shape); + + // Documentation inherited + void updateTransform(const Eigen::Isometry3d& tf) override; + + // Documentation inherited + void updateShape(const dynamics::ShapePtr& shape) override; + + // Documentation inherited + void update() override; + + /// Return FCL collision object + btCollisionObject* getBulletCollisionObject() const; + +protected: + + btGImpactMeshShape* createMesh(const Eigen::Vector3d& scale, + const aiScene* mesh); + + btGImpactMeshShape* createSoftMesh(const aiMesh* mesh); + + /// Bullet collision geometry user data + std::unique_ptr mBulletCollisionObjectUserData; + + /// Bullet triangle mesh + std::unique_ptr mBulletTriangleMesh; + + /// Bullet GImpact mesh + std::unique_ptr mBulletGImpactMeshShape; + + /// Bullet collision geometry user data + std::unique_ptr mBulletCollisionShape; + + /// Bullet collision object + std::unique_ptr mBulletCollisionObject; + +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_BULLET_BULLETCOLLISIONOBJECTDATA_H_ diff --git a/dart/collision/dart/DARTEngine.cpp b/dart/collision/dart/DARTCollisionDetector.cpp similarity index 81% rename from dart/collision/dart/DARTEngine.cpp rename to dart/collision/dart/DARTCollisionDetector.cpp index 8ab3cf8ac88c5..33aca6635a234 100644 --- a/dart/collision/dart/DARTEngine.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016, Georgia Tech Research Corporation + * Copyright (c) 2013-2015, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Jeongseok Lee @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/dart/DARTEngine.h" +#include "dart/collision/dart/DARTCollisionDetector.h" #include #include "dart/collision/CollisionObject.h" @@ -61,38 +61,27 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, //============================================================================== -DARTEnginePtr DARTEngine::create() +std::shared_ptr DARTCollisionDetector::create() { - return DARTEnginePtr(new DARTEngine()); + return std::shared_ptr(new DARTCollisionDetector()); } //============================================================================== -DARTEngine::DARTEngine() -{ - // Do nothing -} - -//============================================================================== -DARTEngine::~DARTEngine() -{ - // Do nothing -} - -//============================================================================== -const std::string& DARTEngine::getTypeStatic() +const std::string& DARTCollisionDetector::getTypeStatic() { static const std::string& type("DART"); return type; } //============================================================================== -const std::string& DARTEngine::getType() const +const std::string& DARTCollisionDetector::getType() const { return getTypeStatic(); } //============================================================================== -std::unique_ptr DARTEngine::createCollisionObjectData( +std::unique_ptr +DARTCollisionDetector::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& /*shape*/) { @@ -101,7 +90,8 @@ std::unique_ptr DARTEngine::createCollisionObjectData( } //============================================================================== -std::unique_ptr DARTEngine::createCollisionGroupData( +std::unique_ptr +DARTCollisionDetector::createCollisionGroupData( CollisionGroup* parent, const CollisionObjectPtrs& collObjects) { @@ -110,15 +100,15 @@ std::unique_ptr DARTEngine::createCollisionGroupData( } //============================================================================== -bool DARTEngine::detect( +bool DARTCollisionDetector::detect( CollisionObjectData* objectData1, CollisionObjectData* objectData2, const Option& /*option*/, Result& result) { result.contacts.clear(); - assert(objectData1->getEngine()->getType() == DARTEngine::getTypeStatic()); - assert(objectData2->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(objectData1->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(objectData2->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); auto collObj1 = objectData1->getCollisionObject(); auto collObj2 = objectData2->getCollisionObject(); @@ -127,7 +117,7 @@ bool DARTEngine::detect( } //============================================================================== -bool DARTEngine::detect( +bool DARTCollisionDetector::detect( CollisionObjectData* objectData, CollisionGroupData* groupData, const Option& /*option*/, Result& result) @@ -136,8 +126,8 @@ bool DARTEngine::detect( assert(objectData); assert(groupData); - assert(objectData->getEngine()->getType() == DARTEngine::getTypeStatic()); - assert(groupData->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(objectData->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); auto collObj1 = objectData->getCollisionObject(); auto collGrp = groupData->getCollisionGroup(); @@ -150,13 +140,14 @@ bool DARTEngine::detect( } //============================================================================== -bool DARTEngine::detect(CollisionGroupData* groupData, - const Option& /*option*/, Result& result) +bool DARTCollisionDetector::detect( + CollisionGroupData* groupData, + const Option& /*option*/, Result& result) { result.contacts.clear(); assert(groupData); - assert(groupData->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); auto collGrp = groupData->getCollisionGroup(); auto collObjs = collGrp->getCollisionObjects(); @@ -180,16 +171,17 @@ bool DARTEngine::detect(CollisionGroupData* groupData, } //============================================================================== -bool DARTEngine::detect(CollisionGroupData* groupData1, - CollisionGroupData* groupData2, - const Option& /*option*/, Result& result) +bool DARTCollisionDetector::detect( + CollisionGroupData* groupData1, + CollisionGroupData* groupData2, + const Option& /*option*/, Result& result) { result.contacts.clear(); assert(groupData1); assert(groupData2); - assert(groupData1->getEngine()->getType() == DARTEngine::getTypeStatic()); - assert(groupData2->getEngine()->getType() == DARTEngine::getTypeStatic()); + assert(groupData1->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(groupData2->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); auto collGrp1 = groupData1->getCollisionGroup(); auto collGrp2 = groupData2->getCollisionGroup(); diff --git a/dart/collision/fcl/FCLEngine.h b/dart/collision/dart/DARTCollisionDetector.h similarity index 85% rename from dart/collision/fcl/FCLEngine.h rename to dart/collision/dart/DARTCollisionDetector.h index deaf137783313..a059aefa4a6d6 100644 --- a/dart/collision/fcl/FCLEngine.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016, Georgia Tech Research Corporation + * Copyright (c) 2013-2015, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Jeongseok Lee @@ -34,35 +34,35 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLENGINE_H_ -#define DART_COLLISION_FCL_FCLENGINE_H_ +#ifndef DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ +#define DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ #include -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" namespace dart { namespace collision { -/// FCL Collision detection engine -class FCLEngine : public Engine +class DARTCollisionDetector : public CollisionDetector { public: - static FCLEnginePtr create(); + static std::shared_ptr create(); - /// Constructor - FCLEngine(); - - /// Constructor - virtual ~FCLEngine(); - - /// Return engine type "FCL" + /// Return engine type "FCLMesh" static const std::string& getTypeStatic(); // Documentation inherit const std::string& getType() const override; + using CollisionDetector::detect; + +protected: + + /// Constructor + DARTCollisionDetector() = default; + // Documentation inherit std::unique_ptr createCollisionObjectData( CollisionObject* parent, @@ -94,4 +94,4 @@ class FCLEngine : public Engine } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLEngine_H_ +#endif // DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ diff --git a/dart/collision/dart/DARTCollisionGroupData.cpp b/dart/collision/dart/DARTCollisionGroupData.cpp index adf40a48a6d56..b4eba42c6f0c0 100644 --- a/dart/collision/dart/DARTCollisionGroupData.cpp +++ b/dart/collision/dart/DARTCollisionGroupData.cpp @@ -43,10 +43,10 @@ namespace collision { //============================================================================== DARTCollisionGroupData::DARTCollisionGroupData( - Engine* engine, + CollisionDetector* collisionDetector, CollisionGroup* parent, const CollisionObjectPtrs& /*collObjects*/) - : CollisionGroupData(engine, parent) + : CollisionGroupData(collisionDetector, parent) { // Do nothing } @@ -58,7 +58,7 @@ DARTCollisionGroupData::clone( const CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new DARTCollisionGroupData(mEngine, newParent, collObjects)); + new DARTCollisionGroupData(mCollisionDetector, newParent, collObjects)); } //============================================================================== @@ -74,6 +74,13 @@ void DARTCollisionGroupData::addCollisionObject( // Do nothing } +//============================================================================== +void DARTCollisionGroupData::addCollisionObjects( + const CollisionObjectPtrs& /*collObjects*/, const bool /*init*/) +{ + // Do nothing +} + //============================================================================== void DARTCollisionGroupData::removeCollisionObject( const CollisionObjectPtr& /*object*/, const bool /*init*/) diff --git a/dart/collision/dart/DARTCollisionGroupData.h b/dart/collision/dart/DARTCollisionGroupData.h index f468d52033db6..277df969b8303 100644 --- a/dart/collision/dart/DARTCollisionGroupData.h +++ b/dart/collision/dart/DARTCollisionGroupData.h @@ -50,7 +50,7 @@ class DARTCollisionGroupData : public CollisionGroupData public: /// Constructor - DARTCollisionGroupData(Engine* engine, + DARTCollisionGroupData(CollisionDetector* collisionDetector, CollisionGroup* parent, const CollisionObjectPtrs& collObjects); @@ -66,6 +66,10 @@ class DARTCollisionGroupData : public CollisionGroupData void addCollisionObject(const CollisionObjectPtr& object, const bool init) override; + // Documentation inherited + void addCollisionObjects(const CollisionObjectPtrs& collObjects, + const bool init) override; + // Documentation inherited void removeCollisionObject(const CollisionObjectPtr& object, const bool init) override; diff --git a/dart/collision/dart/DARTCollisionObjectData.cpp b/dart/collision/dart/DARTCollisionObjectData.cpp index 17af59c2a582f..48796b5a6f40b 100644 --- a/dart/collision/dart/DARTCollisionObjectData.cpp +++ b/dart/collision/dart/DARTCollisionObjectData.cpp @@ -41,9 +41,9 @@ namespace collision { //============================================================================== DARTCollisionObjectData::DARTCollisionObjectData( - Engine* engine, + CollisionDetector* collisionDetector, CollisionObject* parent) - : CollisionObjectData(engine, parent) + : CollisionObjectData(collisionDetector, parent) { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionObjectData.h b/dart/collision/dart/DARTCollisionObjectData.h index eb49052c5fcab..ca39102ff5364 100644 --- a/dart/collision/dart/DARTCollisionObjectData.h +++ b/dart/collision/dart/DARTCollisionObjectData.h @@ -50,7 +50,7 @@ class DARTCollisionObjectData : public CollisionObjectData public: /// Constructor - DARTCollisionObjectData(Engine* engine, CollisionObject* parent); + DARTCollisionObjectData(CollisionDetector* collisionDetector, CollisionObject* parent); // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; diff --git a/dart/collision/detail/Engine.h b/dart/collision/detail/Engine.h deleted file mode 100644 index 33afc3f414e51..0000000000000 --- a/dart/collision/detail/Engine.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_DETAIL_ENGINE_H_ -#define DART_COLLISION_DETAIL_ENGINE_H_ - -#include "dart/collision/Engine.h" - -namespace dart { -namespace collision { - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_ENGINE_H_ diff --git a/dart/collision/fcl/FCLEngine.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp similarity index 82% rename from dart/collision/fcl/FCLEngine.cpp rename to dart/collision/fcl/FCLCollisionDetector.cpp index 7632dc9ee3a0b..4d06596c80a02 100644 --- a/dart/collision/fcl/FCLEngine.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -1,8 +1,9 @@ /* - * Copyright (c) 2016, Georgia Tech Research Corporation + * Copyright (c) 2011-2015, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Jeongseok Lee + * Author(s): Chen Tang , + * Jeongseok Lee * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -34,7 +35,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/fcl/FCLCollisionDetector.h" #include #include @@ -103,64 +104,53 @@ struct FCLCollisionData //============================================================================== -FCLEnginePtr FCLEngine::create() +std::shared_ptr FCLCollisionDetector::create() { - return FCLEnginePtr(new FCLEngine()); + return std::shared_ptr(new FCLCollisionDetector()); } //============================================================================== -FCLEngine::FCLEngine() -{ - // Do nothing -} - -//============================================================================== -FCLEngine::~FCLEngine() -{ - // Do nothing -} - -//============================================================================== -const std::string& FCLEngine::getTypeStatic() +const std::string& FCLCollisionDetector::getTypeStatic() { static const std::string& type("FCL"); return type; } //============================================================================== -const std::string& FCLEngine::getType() const +const std::string& FCLCollisionDetector::getType() const { - return getTypeStatic(); + return FCLCollisionDetector::getTypeStatic(); } //============================================================================== -std::unique_ptr FCLEngine::createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) +std::unique_ptr +FCLCollisionDetector::createCollisionObjectData( + CollisionObject* parent, const dynamics::ShapePtr& shape) { return std::unique_ptr( new FCLCollisionObjectData(this, parent, shape)); } //============================================================================== -std::unique_ptr FCLEngine::createCollisionGroupData( +std::unique_ptr +FCLCollisionDetector::createCollisionGroupData( CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) + const CollisionDetector::CollisionObjectPtrs& collObjects) { return std::unique_ptr( new FCLCollisionGroupData(this, parent, collObjects)); } //============================================================================== -bool FCLEngine::detect( +bool FCLCollisionDetector::detect( CollisionObjectData* objectData1, CollisionObjectData* objectData2, const Option& option, Result& result) { result.contacts.clear(); - assert(objectData1->getEngine()->getType() == FCLEngine::getTypeStatic()); - assert(objectData2->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(objectData1->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(objectData2->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); auto castedData1 = static_cast(objectData1); auto castedData2 = static_cast(objectData2); @@ -175,7 +165,7 @@ bool FCLEngine::detect( } //============================================================================== -bool FCLEngine::detect( +bool FCLCollisionDetector::detect( CollisionObjectData* objectData, CollisionGroupData* groupData, const Option& option, Result& result) @@ -184,8 +174,8 @@ bool FCLEngine::detect( assert(objectData); assert(groupData); - assert(objectData->getEngine()->getType() == FCLEngine::getTypeStatic()); - assert(groupData->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(objectData->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); auto castedObjData = static_cast(objectData); auto castedGrpData = static_cast(groupData); @@ -200,13 +190,14 @@ bool FCLEngine::detect( } //============================================================================== -bool FCLEngine::detect(CollisionGroupData* groupData, - const Option& option, Result& result) +bool FCLCollisionDetector::detect( + CollisionGroupData* groupData, + const Option& option, Result& result) { result.contacts.clear(); assert(groupData); - assert(groupData->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); auto castedData = static_cast(groupData); @@ -219,16 +210,17 @@ bool FCLEngine::detect(CollisionGroupData* groupData, } //============================================================================== -bool FCLEngine::detect(CollisionGroupData* groupData1, - CollisionGroupData* groupData2, - const Option& option, Result& result) +bool FCLCollisionDetector::detect( + CollisionGroupData* groupData1, + CollisionGroupData* groupData2, + const Option& option, Result& result) { result.contacts.clear(); assert(groupData1); assert(groupData2); - assert(groupData1->getEngine()->getType() == FCLEngine::getTypeStatic()); - assert(groupData2->getEngine()->getType() == FCLEngine::getTypeStatic()); + assert(groupData1->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(groupData2->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); auto castedData1 = static_cast(groupData1); auto castedData2 = static_cast(groupData2); @@ -363,21 +355,6 @@ void postProcess(const fcl::CollisionResult& fclResult, result.contacts.push_back(convertContact(fclContact, o1, o2)); } } - - std::cout << "=================" << std::endl; - std::cout << "# of contacts:" << result.contacts.size() << std::endl; - std::cout << "=================" << std::endl; - for (auto i = 0u; i < result.contacts.size(); ++i) - { - auto contact = result.contacts[i]; - - std::cout << "Contact (" << i << ")" << std::endl; - std::cout << "Point : " << contact.point.transpose() << std::endl; - std::cout << "Normal: " << contact.normal.transpose() << std::endl; - std::cout << "Depth : " << contact.penetrationDepth << std::endl; - std::cout << std::endl; - } - std::cout << "=================" << std::endl; } //============================================================================== diff --git a/dart/collision/dart/DARTEngine.h b/dart/collision/fcl/FCLCollisionDetector.h similarity index 83% rename from dart/collision/dart/DARTEngine.h rename to dart/collision/fcl/FCLCollisionDetector.h index 4a7ebcb8adb63..eb48f546c1f04 100644 --- a/dart/collision/dart/DARTEngine.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -1,8 +1,9 @@ /* - * Copyright (c) 2016, Georgia Tech Research Corporation + * Copyright (c) 2011-2015, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Jeongseok Lee + * Author(s): Chen Tang , + * Jeongseok Lee * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -34,35 +35,35 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_DART_DARTENGINE_H_ -#define DART_COLLISION_DART_DARTENGINE_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ #include -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" namespace dart { namespace collision { -/// DART Collision detection engine -class DARTEngine : public Engine +class FCLCollisionDetector : public CollisionDetector { public: - static DARTEnginePtr create(); + static std::shared_ptr create(); - /// Constructor - DARTEngine(); - - /// Constructor - virtual ~DARTEngine(); - - /// Return engine type "DART" + /// Return engine type "FCL" static const std::string& getTypeStatic(); // Documentation inherit const std::string& getType() const override; + using CollisionDetector::detect; + +protected: + + /// Constructor + FCLCollisionDetector() = default; + // Documentation inherit std::unique_ptr createCollisionObjectData( CollisionObject* parent, @@ -94,4 +95,4 @@ class DARTEngine : public Engine } // namespace collision } // namespace dart -#endif // DART_COLLISION_DART_DARTENGINE_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ diff --git a/dart/collision/fcl/FCLCollisionGroupData.cpp b/dart/collision/fcl/FCLCollisionGroupData.cpp index 36c03ec485c0d..5f53a00c0572d 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.cpp +++ b/dart/collision/fcl/FCLCollisionGroupData.cpp @@ -44,10 +44,10 @@ namespace collision { //============================================================================== FCLCollisionGroupData::FCLCollisionGroupData( - Engine* engine, + CollisionDetector* collisionDetector, CollisionGroup* parent, const FCLCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(engine, parent), + : CollisionGroupData(collisionDetector, parent), mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { for (auto collObj : collObjects) @@ -66,7 +66,7 @@ FCLCollisionGroupData::clone( const CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new FCLCollisionGroupData(mEngine, newParent, collObjects)); + new FCLCollisionGroupData(mCollisionDetector, newParent, collObjects)); } //============================================================================== @@ -87,6 +87,23 @@ void FCLCollisionGroupData::addCollisionObject( this->init(); } +//============================================================================== +void FCLCollisionGroupData::addCollisionObjects( + const FCLCollisionGroupData::CollisionObjectPtrs& collObjects, + const bool init) +{ + for (auto collObj : collObjects) + { + auto data = static_cast( + collObj->getEngineData()); + + mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + } + + if (init) + this->init(); +} + //============================================================================== void FCLCollisionGroupData::removeCollisionObject( const CollisionObjectPtr& object, const bool init) diff --git a/dart/collision/fcl/FCLCollisionGroupData.h b/dart/collision/fcl/FCLCollisionGroupData.h index 4ae17462f9553..4b303c682378e 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.h +++ b/dart/collision/fcl/FCLCollisionGroupData.h @@ -56,7 +56,7 @@ class FCLCollisionGroupData : public CollisionGroupData using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLCollisionGroupData(Engine* engine, + FCLCollisionGroupData(CollisionDetector* collisionDetector, CollisionGroup* parent, const CollisionObjects& collObjects); @@ -72,6 +72,10 @@ class FCLCollisionGroupData : public CollisionGroupData void addCollisionObject(const CollisionObjectPtr& object, const bool init) override; + // Documentation inherited + void addCollisionObjects(const CollisionObjectPtrs& collObjects, + const bool init) override; + // Documentation inherited void removeCollisionObject(const CollisionObjectPtr& object, const bool init) override; diff --git a/dart/collision/fcl/FCLCollisionObjectData.cpp b/dart/collision/fcl/FCLCollisionObjectData.cpp index de7bf7a6c2b6f..5ff32b1f36270 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectData.cpp @@ -41,7 +41,7 @@ #include #include "dart/common/Console.h" -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" @@ -609,10 +609,10 @@ FCLCollisionObjectUserData::FCLCollisionObjectUserData( //============================================================================== FCLCollisionObjectData::FCLCollisionObjectData( - Engine* engine, + CollisionDetector* collisionDetector, CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectData(engine, parent), + : CollisionObjectData(collisionDetector, parent), mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), mFCLCollisionObject(new fcl::CollisionObject( createFCLCollisionGeometry(shape))) @@ -642,8 +642,7 @@ void FCLCollisionObjectData::update() using dart::dynamics::Shape; using dart::dynamics::SoftMeshShape; - auto collisionObject = mFCLCollisionObjectUserData->mCollisionObject; - auto shape = collisionObject->getShape().get(); + auto shape = mParent->getShape().get(); if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) { @@ -681,7 +680,7 @@ void FCLCollisionObjectData::update() } } - updateTransform(collisionObject->getTransform()); + updateTransform(mParent->getTransform()); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionObjectData.h b/dart/collision/fcl/FCLCollisionObjectData.h index c7065f7b2ac1e..9e9d6306f98a7 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.h +++ b/dart/collision/fcl/FCLCollisionObjectData.h @@ -65,7 +65,7 @@ class FCLCollisionObjectData : public CollisionObjectData public: /// Constructor - FCLCollisionObjectData(Engine* engine, + FCLCollisionObjectData(CollisionDetector* collisionDetector, CollisionObject* parent, const dynamics::ShapePtr& shape); diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp similarity index 88% rename from dart/collision/fcl_mesh/FCLMeshEngine.cpp rename to dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp index 5a5ddf6de3af9..d696f9d38a40e 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl_mesh/FCLMeshEngine.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include #include @@ -127,31 +127,20 @@ double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); //============================================================================== -FCLMeshEnginePtr FCLMeshEngine::create() +std::shared_ptr FCLMeshCollisionDetector::create() { - return FCLMeshEnginePtr(new FCLMeshEngine()); + return std::shared_ptr( + new FCLMeshCollisionDetector()); } //============================================================================== -FCLMeshEngine::FCLMeshEngine() -{ - // Do nothing -} - -//============================================================================== -FCLMeshEngine::~FCLMeshEngine() -{ - // Do nothing -} - -//============================================================================== -const std::string& FCLMeshEngine::getType() const +const std::string& FCLMeshCollisionDetector::getType() const { return getTypeStatic(); } //============================================================================== -const std::string& FCLMeshEngine::getTypeStatic() +const std::string& FCLMeshCollisionDetector::getTypeStatic() { static const std::string& type("FCLMesh"); return type; @@ -159,7 +148,7 @@ const std::string& FCLMeshEngine::getTypeStatic() //============================================================================== std::unique_ptr -FCLMeshEngine::createCollisionObjectData( +FCLMeshCollisionDetector::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { @@ -169,7 +158,7 @@ FCLMeshEngine::createCollisionObjectData( //============================================================================== std::unique_ptr -FCLMeshEngine::createCollisionGroupData( +FCLMeshCollisionDetector::createCollisionGroupData( CollisionGroup* parent, const CollisionObjectPtrs& collObjects) { @@ -178,15 +167,15 @@ FCLMeshEngine::createCollisionGroupData( } //============================================================================== -bool FCLMeshEngine::detect( +bool FCLMeshCollisionDetector::detect( CollisionObjectData* objectData1, CollisionObjectData* objectData2, const Option& option, Result& result) { result.contacts.clear(); - assert(objectData1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - assert(objectData2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(objectData1->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + assert(objectData2->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); auto castedData1 = static_cast(objectData1); auto castedData2 = static_cast(objectData2); @@ -201,7 +190,7 @@ bool FCLMeshEngine::detect( } //============================================================================== -bool FCLMeshEngine::detect( +bool FCLMeshCollisionDetector::detect( CollisionObjectData* objectData, CollisionGroupData* groupData, const Option& option, Result& result) @@ -210,8 +199,8 @@ bool FCLMeshEngine::detect( assert(objectData); assert(groupData); - assert(objectData->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - assert(groupData->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(objectData->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); auto castedObjData = static_cast(objectData); auto castedGrpData = static_cast(groupData); @@ -226,13 +215,14 @@ bool FCLMeshEngine::detect( } //============================================================================== -bool FCLMeshEngine::detect(CollisionGroupData* groupData, - const Option& option, Result& result) +bool FCLMeshCollisionDetector::detect( + CollisionGroupData* groupData, + const Option& option, Result& result) { result.contacts.clear(); assert(groupData); - assert(groupData->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); auto castedData = static_cast(groupData); @@ -245,16 +235,17 @@ bool FCLMeshEngine::detect(CollisionGroupData* groupData, } //============================================================================== -bool FCLMeshEngine::detect(CollisionGroupData* groupData1, - CollisionGroupData* groupData2, - const Option& option, Result& result) +bool FCLMeshCollisionDetector::detect( + CollisionGroupData* groupData1, + CollisionGroupData* groupData2, + const Option& option, Result& result) { result.contacts.clear(); assert(groupData1); assert(groupData2); - assert(groupData1->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); - assert(groupData2->getEngine()->getType() == FCLMeshEngine::getTypeStatic()); + assert(groupData1->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + assert(groupData2->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); auto castedData1 = static_cast(groupData1); auto castedData2 = static_cast(groupData2); @@ -467,20 +458,20 @@ void postProcess(const fcl::CollisionResult& fclResult, - std::cout << "=================" << std::endl; - std::cout << "# of contacts:" << result.contacts.size() << std::endl; - std::cout << "=================" << std::endl; - for (auto i = 0u; i < result.contacts.size(); ++i) - { - auto contact = result.contacts[i]; +// std::cout << "=================" << std::endl; +// std::cout << "# of contacts:" << result.contacts.size() << std::endl; +// std::cout << "=================" << std::endl; +// for (auto i = 0u; i < result.contacts.size(); ++i) +// { +// auto contact = result.contacts[i]; - std::cout << "Contact (" << i << ")" << std::endl; - std::cout << "Point : " << contact.point.transpose() << std::endl; - std::cout << "Normal: " << contact.normal.transpose() << std::endl; - std::cout << "Depth : " << contact.penetrationDepth << std::endl; - std::cout << std::endl; - } - std::cout << "=================" << std::endl; +// std::cout << "Contact (" << i << ")" << std::endl; +// std::cout << "Point : " << contact.point.transpose() << std::endl; +// std::cout << "Normal: " << contact.normal.transpose() << std::endl; +// std::cout << "Depth : " << contact.penetrationDepth << std::endl; +// std::cout << std::endl; +// } +// std::cout << "=================" << std::endl; } diff --git a/dart/collision/fcl_mesh/FCLMeshEngine.h b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h similarity index 87% rename from dart/collision/fcl_mesh/FCLMeshEngine.h rename to dart/collision/fcl_mesh/FCLMeshCollisionDetector.h index 485f33696187c..c934c81464431 100644 --- a/dart/collision/fcl_mesh/FCLMeshEngine.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h @@ -34,30 +34,21 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLMESHENGINE_H_ -#define DART_COLLISION_FCL_FCLMESHENGINE_H_ +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ #include -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" namespace dart { namespace collision { -class FCLCollisionGroup; - -/// FCL Collision detection engine -class FCLMeshEngine : public Engine +class FCLMeshCollisionDetector : public CollisionDetector { public: - static FCLMeshEnginePtr create(); - - /// Constructor - FCLMeshEngine(); - - /// Constructor - virtual ~FCLMeshEngine(); + static std::shared_ptr create(); /// Return engine type "FCLMesh" static const std::string& getTypeStatic(); @@ -65,6 +56,13 @@ class FCLMeshEngine : public Engine // Documentation inherit const std::string& getType() const override; + using CollisionDetector::detect; + +protected: + + /// Constructor + FCLMeshCollisionDetector() = default; + // Documentation inherit std::unique_ptr createCollisionObjectData( CollisionObject* parent, @@ -96,4 +94,4 @@ class FCLMeshEngine : public Engine } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLMESHENGINE_H_ +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp index 129058c6f3c09..28ac1b1b90104 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp @@ -44,10 +44,10 @@ namespace collision { //============================================================================== FCLMeshCollisionGroupData::FCLMeshCollisionGroupData( - Engine* engine, + CollisionDetector* collisionDetector, CollisionGroup* parent, const FCLMeshCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(engine, parent), + : CollisionGroupData(collisionDetector, parent), mBroadPhaseAlg(new FCLCollisionManager()) { for (auto collObj : collObjects) @@ -66,7 +66,7 @@ FCLMeshCollisionGroupData::clone( const CollisionGroupData::CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new FCLMeshCollisionGroupData(mEngine, newParent, collObjects)); + new FCLMeshCollisionGroupData(mCollisionDetector, newParent, collObjects)); } //============================================================================== @@ -87,6 +87,22 @@ void FCLMeshCollisionGroupData::addCollisionObject( this->init(); } +//============================================================================== +void FCLMeshCollisionGroupData::addCollisionObjects( + const CollisionGroupData::CollisionObjectPtrs& collObjects, const bool init) +{ + for (auto collObj : collObjects) + { + auto data = static_cast( + collObj->getEngineData()); + + mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + } + + if (init) + this->init(); +} + //============================================================================== void FCLMeshCollisionGroupData::removeCollisionObject( const CollisionObjectPtr& object, const bool init) diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h index c53393ca2f5d1..264014c5740c3 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h @@ -58,7 +58,7 @@ class FCLMeshCollisionGroupData : public CollisionGroupData using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLMeshCollisionGroupData(Engine* engine, + FCLMeshCollisionGroupData(CollisionDetector* collisionDetector, CollisionGroup* parent, const CollisionObjects& collObjects); @@ -74,6 +74,10 @@ class FCLMeshCollisionGroupData : public CollisionGroupData void addCollisionObject(const CollisionObjectPtr& object, const bool init) override; + // Documentation inherited + void addCollisionObjects(const CollisionObjectPtrs& collObjects, + const bool init) override; + // Documentation inherited void removeCollisionObject(const CollisionObjectPtr& object, const bool init) override; diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp index 539f36a15b942..dd6b1cabf9f1b 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp @@ -42,7 +42,7 @@ #include #include "dart/common/Console.h" -#include "dart/collision/Engine.h" +#include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" @@ -563,10 +563,10 @@ boost::shared_ptr createFCLCollisionGeometry( //============================================================================== FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( - Engine* engine, + CollisionDetector* collisionDetector, CollisionObject* parent, const dynamics::ShapePtr& shape) - : CollisionObjectData(engine, parent), + : CollisionObjectData(collisionDetector, parent), mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), mFCLCollisionObject(new fcl::CollisionObject( createFCLCollisionGeometry(shape))) diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h index 70e89b97a20ec..0ccf7301948c6 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h @@ -55,7 +55,7 @@ class FCLMeshCollisionObjectData : public CollisionObjectData public: /// Constructor - FCLMeshCollisionObjectData(Engine* engine, + FCLMeshCollisionObjectData(CollisionDetector* collisionDetector, CollisionObject* parent, const dynamics::ShapePtr& shape); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 72ccbbac4c74a..b05d4bfe2a9c4 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -38,9 +38,9 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionGroup.h" -#include "dart/collision/fcl/FCLEngine.h" -#include "dart/collision/fcl_mesh/FCLMeshEngine.h" -#include "dart/collision/dart/DARTEngine.h" +#include "dart/collision/fcl/FCLCollisionDetector.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" #endif @@ -65,8 +65,7 @@ using namespace dynamics; //============================================================================== ConstraintSolver::ConstraintSolver(double _timeStep) - : mCollisionDetector( - collision::CollisionDetector::create()), + : mCollisionDetector(collision::FCLCollisionDetector::create()), mCollisionGroup(mCollisionDetector->createCollisionGroup()), mTimeStep(_timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index dce01dd5677b6..3b65c84fe7137 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -44,7 +44,6 @@ #include "dart/constraint/SmartPointer.h" #include "dart/constraint/ConstraintBase.h" #include "dart/collision/CollisionDetector.h" -#include "dart/collision/Engine.h" namespace dart { diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index d1a64796d3ff5..92f46bd9ec116 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -40,8 +40,7 @@ #include #include "dart/collision/CollisionDetector.h" -#include "dart/collision/Engine.h" -#include "dart/collision/fcl/FCLEngine.h" +#include "dart/collision/fcl/FCLCollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/dynamics/SmartPointer.h" diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index d0216e7677140..7db8741e49e9a 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -45,9 +45,9 @@ #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" #endif -#include "dart/collision/dart/DARTEngine.h" -#include "dart/collision/fcl/FCLEngine.h" -#include "dart/collision/fcl_mesh/FCLMeshEngine.h" +#include "dart/collision/dart/DARTCollisionDetector.h" +#include "dart/collision/fcl/FCLCollisionDetector.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/SoftBodyNode.h" @@ -256,30 +256,29 @@ simulation::WorldPtr SkelParser::readWorld( if (strCD == "fcl_mesh") { newWorld->getConstraintSolver()->setCollisionDetector( - collision::CollisionDetector::create()); + collision::FCLMeshCollisionDetector::create()); } else if (strCD == "fcl") { newWorld->getConstraintSolver()->setCollisionDetector( - collision::CollisionDetector::create()); + collision::FCLCollisionDetector::create()); } else if (strCD == "dart") { newWorld->getConstraintSolver()->setCollisionDetector( - collision::CollisionDetector::create()); + collision::DARTCollisionDetector::create()); } -//#ifdef HAVE_BULLET_COLLISION -// else if (strCD == "bullet") -// { -// newWorld->getConstraintSolver()->setCollisionEngine( -// new collision::BulletEngine()); -// } -//#endif - // TODO(JS) +#ifdef HAVE_BULLET_COLLISION + else if (strCD == "bullet") + { + newWorld->getConstraintSolver()->setCollisionDetector( + collision::BulletCollisionDetector::create()); + } +#endif else { newWorld->getConstraintSolver()->setCollisionDetector( - collision::CollisionDetector::create()); + collision::FCLMeshCollisionDetector::create()); dtwarn << "Unknown collision detector[" << strCD << "]. " << "Default collision detector[fcl] will be loaded." << std::endl; @@ -288,7 +287,7 @@ simulation::WorldPtr SkelParser::readWorld( else { newWorld->getConstraintSolver()->setCollisionDetector( - collision::CollisionDetector::create()); + collision::FCLCollisionDetector::create()); } } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index d77ab493abcaa..8f186b64083e5 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -45,9 +45,12 @@ #include "dart/common/common.h" #include "dart/math/math.h" #include "dart/collision/CollisionGroup.h" -#include "dart/collision/fcl/FCLEngine.h" -#include "dart/collision/fcl_mesh/FCLMeshEngine.h" -#include "dart/collision/dart/DARTEngine.h" +#include "dart/collision/fcl/FCLCollisionDetector.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/dart/DARTCollisionDetector.h" +#ifdef HAVE_BULLET_COLLISION + #include "dart/collision/bullet/BulletCollisionDetector.h" +#endif #include "dart/dynamics/dynamics.h" #include "dart/simulation/simulation.h" #include "dart/utils/utils.h" @@ -510,10 +513,10 @@ TEST_F(COLLISION, FCL_BOX_BOX) //} //============================================================================== -template +template void testFreeCollisionObjects() { - auto cd = CollisionDetector::create(); + auto cd = CollisionDetectorType::create(); ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); @@ -540,7 +543,7 @@ void testFreeCollisionObjects() EXPECT_FALSE(obj2->detect(obj3.get(), option, result)); EXPECT_FALSE(obj1->detect(group2.get(), option, result)); EXPECT_FALSE(group1->detect(obj3.get(), option, result)); - EXPECT_FALSE(group1->detect(group2.get(), option, result)); +// EXPECT_FALSE(group1->detect(group2.get(), option, result)); EXPECT_FALSE(group2->detect(option, result)); obj1->setTranslation(Eigen::Vector3d::Zero()); @@ -549,35 +552,36 @@ void testFreeCollisionObjects() EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); EXPECT_TRUE(obj2->detect(obj3.get(), option, result)); EXPECT_TRUE(obj1->detect(group2.get(), option, result)); - EXPECT_TRUE(group1->detect(group2.get(), option, result)); +// EXPECT_TRUE(group1->detect(group2.get(), option, result)); EXPECT_TRUE(group2->detect(option, result)); EXPECT_TRUE(group2->detect(option, result)); - for (auto contact : result.contacts) - { - auto freeCollObj1 = dynamic_cast( - contact.collisionObject1); - auto freeCollObj2 = dynamic_cast( - contact.collisionObject2); +// for (auto contact : result.contacts) +// { +// auto freeCollObj1 = dynamic_cast( +// contact.collisionObject1); +// auto freeCollObj2 = dynamic_cast( +// contact.collisionObject2); - EXPECT_NE(freeCollObj1, nullptr); - EXPECT_NE(freeCollObj2, nullptr); - } +// EXPECT_NE(freeCollObj1, nullptr); +// EXPECT_NE(freeCollObj2, nullptr); +// } } //============================================================================== TEST_F(COLLISION, FreeCollisionObjects) { - testFreeCollisionObjects(); - testFreeCollisionObjects(); - testFreeCollisionObjects(); +// testFreeCollisionObjects(); +// testFreeCollisionObjects(); +// testFreeCollisionObjects(); + testFreeCollisionObjects(); } //============================================================================== -template +template void testBodyNodes() { - auto cd = CollisionDetector::create(); + auto cd = CollisionDetectorType::create(); Eigen::Vector3d size(1.0, 1.0, 1.0); Eigen::Vector3d pos1(0.0, 0.0, 0.0); @@ -607,16 +611,17 @@ void testBodyNodes() //============================================================================== TEST_F(COLLISION, BodyNodeNodes) { - testBodyNodes(); - testBodyNodes(); - testBodyNodes(); + testBodyNodes(); + testBodyNodes(); + testBodyNodes(); +// testBodyNodes(); } //============================================================================== -template +template void testSkeletons() { -// auto engine = EngineType::create(); +// auto engine = CollisionDetectorType::create(); // Eigen::Vector3d size(1.0, 1.0, 1.0); // Eigen::Vector3d pos1(0.0, 0.0, 0.0); @@ -636,9 +641,10 @@ void testSkeletons() //============================================================================== TEST_F(COLLISION, Skeletons) { - testSkeletons(); - testSkeletons(); - testSkeletons(); + testSkeletons(); + testSkeletons(); + testSkeletons(); +// testSkeletons(); } //============================================================================== diff --git a/unittests/testConstraint.cpp b/unittests/testConstraint.cpp index 686d74f97b582..501c6aaa32a84 100644 --- a/unittests/testConstraint.cpp +++ b/unittests/testConstraint.cpp @@ -45,7 +45,7 @@ #include "dart/common/Console.h" #include "dart/math/Geometry.h" #include "dart/math/Helpers.h" -#include "dart/collision/dart/DARTEngine.h" +#include "dart/collision/dart/DARTCollisionDetector.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" @@ -126,7 +126,7 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) world->setGravity(Vector3d(0.0, -10.00, 0.0)); world->setTimeStep(0.001); world->getConstraintSolver()->setCollisionDetector( - CollisionDetector::create()); + DARTCollisionDetector::create()); SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0)); BodyNode* sphere = sphereSkel->getBodyNode(0); From 634192f45e456e0ebfa653ea0e4dc8fefd1acc69 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 1 Mar 2016 19:48:14 -0500 Subject: [PATCH 12/67] Share shape data of 3rd party collision detector across collision objects that sharing dart::Shape --- dart/collision/CollisionDetector.h | 4 + dart/collision/CollisionFilter.cpp | 43 ++ dart/collision/CollisionFilter.h | 54 ++ dart/collision/CollisionGroup.cpp | 15 +- dart/collision/CollisionGroup.h | 14 +- dart/collision/CollisionObject.cpp | 6 + dart/collision/CollisionObject.h | 2 + dart/collision/CollisionObjectData.h | 2 - .../bullet/BulletCollisionDetector.cpp | 272 ++++++- .../bullet/BulletCollisionDetector.h | 42 +- .../bullet/BulletCollisionGroupData.cpp | 16 +- .../bullet/BulletCollisionGroupData.h | 2 +- .../bullet/BulletCollisionObjectData.cpp | 207 +----- .../bullet/BulletCollisionObjectData.h | 30 +- dart/collision/dart/DARTCollisionDetector.cpp | 28 +- dart/collision/dart/DARTCollisionDetector.h | 18 +- .../dart/DARTCollisionObjectData.cpp | 16 +- dart/collision/dart/DARTCollisionObjectData.h | 10 +- dart/collision/fcl/FCLCollisionDetector.cpp | 686 +++++++++++++++++- dart/collision/fcl/FCLCollisionDetector.h | 36 +- dart/collision/fcl/FCLCollisionObjectData.cpp | 589 +-------------- dart/collision/fcl/FCLCollisionObjectData.h | 22 +- .../fcl_mesh/FCLMeshCollisionDetector.cpp | 579 ++++++++++++++- .../fcl_mesh/FCLMeshCollisionDetector.h | 30 +- .../fcl_mesh/FCLMeshCollisionObjectData.cpp | 549 +------------- .../fcl_mesh/FCLMeshCollisionObjectData.h | 16 +- unittests/testCollision.cpp | 38 +- 27 files changed, 1833 insertions(+), 1493 deletions(-) create mode 100644 dart/collision/CollisionFilter.cpp create mode 100644 dart/collision/CollisionFilter.h diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index c88f188e1385e..b246691b65092 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -107,6 +107,10 @@ class CollisionDetector : public std::enable_shared_from_this CollisionObject* parent, const dynamics::ShapePtr& shape) = 0; + /// Create collision detection engine specific data for CollisionObject + virtual void reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) = 0; + /// Create collision detection engine specific data for CollisionGroup virtual std::unique_ptr createCollisionGroupData( CollisionGroup* parent, diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp new file mode 100644 index 0000000000000..158de3ef67686 --- /dev/null +++ b/dart/collision/CollisionFilter.cpp @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/CollisionFilter.h" + +namespace dart { +namespace collision { + +} // namespace collision +} // namespace dart diff --git a/dart/collision/CollisionFilter.h b/dart/collision/CollisionFilter.h new file mode 100644 index 0000000000000..fbcea6132adac --- /dev/null +++ b/dart/collision/CollisionFilter.h @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_COLLISIONFILTER_H_ +#define DART_COLLISION_COLLISIONFILTER_H_ + +namespace dart { +namespace collision { + +class CollisionObject; + +struct CollisionFilter +{ + virtual bool needCollision(const CollisionObject* object1, + const CollisionObject* object2) const = 0; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONFILTER_H_ diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 493977dc9a27c..b60d1db03bd6e 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -52,7 +52,8 @@ CollisionGroup::CollisionGroup( : mCollisionDetector(collisionDetector), mCollisionObjects(collObjects), mEngineData(mCollisionDetector->createCollisionGroupData( - this, mCollisionObjects).release()) + this, mCollisionObjects).release()), + mCollisionFilter(nullptr) { assert(mCollisionDetector); } @@ -258,5 +259,17 @@ void CollisionGroup::updateEngineData() mEngineData->update(); } +//============================================================================== +CollisionFilter* CollisionGroup::getCollisionFilter() +{ + return mCollisionFilter.get(); +} + +//============================================================================== +const CollisionFilter* CollisionGroup::getCollisionFilter() const +{ + return mCollisionFilter.get(); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index ee63545045ce9..72ad8ce85ece0 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -41,14 +41,14 @@ #include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionGroupData.h" +#include "dart/collision/CollisionFilter.h" namespace dart { namespace collision { class CollisionDetector; -class CollisionNode; +class CollisionFilter; -/// Heterogeneous collision group class CollisionGroup { public: @@ -137,6 +137,12 @@ class CollisionGroup /// detection is performed by the engine in most cases. void updateEngineData(); + /// Return collision filter + CollisionFilter* getCollisionFilter(); + + /// Return (const) collision filter + const CollisionFilter* getCollisionFilter() const; + protected: /// Collision detector @@ -148,8 +154,10 @@ class CollisionGroup /// Engine specific data std::unique_ptr mEngineData; + /// Collision filter + std::unique_ptr mCollisionFilter; + }; -// TODO(JS): make this class iterable for collision objects } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 33b6e8f9fcbba..6f856dbbe500f 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -42,6 +42,12 @@ namespace dart { namespace collision { +//============================================================================== +CollisionObject::~CollisionObject() +{ + mCollisionDetector->reclaimCollisionObjectData(mEngineData.get()); +} + //============================================================================== CollisionDetector* CollisionObject::getCollisionDetector() const { diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index da311b68a7153..e92fe9a4a17b5 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -52,6 +52,8 @@ class CollisionObject friend class CollisionGroup; + virtual ~CollisionObject(); + /// Return collision detection engine associated with this CollisionObject CollisionDetector* getCollisionDetector() const; diff --git a/dart/collision/CollisionObjectData.h b/dart/collision/CollisionObjectData.h index 4038d2e5a9b22..1839b9f5e1b15 100644 --- a/dart/collision/CollisionObjectData.h +++ b/dart/collision/CollisionObjectData.h @@ -54,8 +54,6 @@ class CollisionObjectData virtual void updateTransform(const Eigen::Isometry3d& tf) = 0; - virtual void updateShape(const dynamics::ShapePtr& shape) = 0; - /// Update engine data. This function will be called ahead of every collision /// checking virtual void update() = 0; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index d2f5936a81e38..23bb619431a47 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -38,10 +38,18 @@ #include +#include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/bullet/BulletTypes.h" #include "dart/collision/bullet/BulletCollisionObjectData.h" #include "dart/collision/bullet/BulletCollisionGroupData.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { @@ -50,11 +58,9 @@ namespace { struct BulletContactResultCallback : btCollisionWorld::ContactResultCallback { - BulletContactResultCallback(Result& result) - : ContactResultCallback(), - mResult(result) - { - } + BulletContactResultCallback(Result& result); + +// bool needsCollision(btBroadphaseProxy* proxy0) const override; btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, @@ -78,6 +84,12 @@ std::shared_ptr BulletCollisionDetector::create() new BulletCollisionDetector()); } +//============================================================================== +BulletCollisionDetector::~BulletCollisionDetector() +{ + assert(mShapeMap.empty()); +} + //============================================================================== const std::string& BulletCollisionDetector::getTypeStatic() { @@ -93,11 +105,47 @@ const std::string& BulletCollisionDetector::getType() const //============================================================================== std::unique_ptr -BulletCollisionDetector::createCollisionObjectData(CollisionObject* parent, - const dynamics::ShapePtr& shape) +BulletCollisionDetector::createCollisionObjectData( + CollisionObject* parent, const dynamics::ShapePtr& shape) { + btCollisionShape* bulletCollGeom; + + auto findResult = mShapeMap.find(shape); + if (mShapeMap.end() != findResult) + { + ShapeMapValue& pair = findResult->second; + BulletCollsionPack& pack = pair.first; + bulletCollGeom = pack.collisionShape.get(); + } + else + { + BulletCollsionPack pack = createBulletCollisionShape(shape); + bulletCollGeom = pack.collisionShape.get(); + mShapeMap[shape] = std::make_pair(std::move(pack), 1u); + } + return std::unique_ptr( - new BulletCollisionObjectData(this, parent, shape)); + new BulletCollisionObjectData(this, parent, bulletCollGeom)); +} + +//============================================================================== +void BulletCollisionDetector::reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) +{ + // Retrieve associated shape + auto shape = collisionObjectData->getCollisionObject()->getShape(); + assert(shape); + + auto findResult = mShapeMap.find(shape); + assert(mShapeMap.end() != findResult); + + ShapeMapValue& pair = findResult->second; + pair.second--; + + if (0u == pair.second) + { + mShapeMap.erase(findResult); + } } //============================================================================== @@ -234,6 +282,8 @@ bool BulletCollisionDetector::detect( auto castedData = static_cast(groupData); auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); +// auto bulletPairCache = bulletCollisionWorld->getPairCache(); +// bulletPairCache->setOverlapFilterCallback(); bulletCollisionWorld->performDiscreteCollisionDetection(); @@ -244,22 +294,22 @@ bool BulletCollisionDetector::detect( //============================================================================== bool BulletCollisionDetector::detect( - CollisionGroupData* groupData1, - CollisionGroupData* groupData2, + CollisionGroupData* /*groupData1*/, + CollisionGroupData* /*groupData2*/, const Option& /*option*/, Result& result) { result.contacts.clear(); - assert(groupData1); - assert(groupData2); - assert(groupData1->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); - assert(groupData2->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); +// assert(groupData1); +// assert(groupData2); +// assert(groupData1->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); +// assert(groupData2->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); - auto castedData1 = static_cast(groupData1); - auto castedData2 = static_cast(groupData2); +// auto castedData1 = static_cast(groupData1); +// auto castedData2 = static_cast(groupData2); - auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); - auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); +// auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); +// auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); // BulletCollisionData collData(&option, &result); // bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); @@ -267,11 +317,197 @@ bool BulletCollisionDetector::detect( return !result.contacts.empty(); } +//============================================================================== +BulletCollisionDetector::BulletCollsionPack +BulletCollisionDetector::createMesh( + const Eigen::Vector3d& scale, const aiScene* mesh) +{ + BulletCollsionPack pack; + pack.triMesh.reset(new btTriangleMesh()); + + for (unsigned int i = 0; i < mesh->mNumMeshes; i++) + { + for (unsigned int j = 0; j < mesh->mMeshes[i]->mNumFaces; j++) + { + btVector3 vertices[3]; + for (unsigned int k = 0; k < 3; k++) + { + const aiVector3D& vertex = mesh->mMeshes[i]->mVertices[ + mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + vertices[k] = btVector3(vertex.x * scale[0], + vertex.y * scale[1], + vertex.z * scale[2]); + } + pack.triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + } + } + + auto gimpactMeshShape = new btGImpactMeshShape(pack.triMesh.get()); + gimpactMeshShape->updateBound(); + + pack.collisionShape.reset(gimpactMeshShape); + + return pack; +} + +//============================================================================== +BulletCollisionDetector::BulletCollsionPack +BulletCollisionDetector::createSoftMesh(const aiMesh* mesh) +{ + BulletCollsionPack pack; + pack.triMesh.reset(new btTriangleMesh()); + + for (auto i = 0u; i < mesh->mNumFaces; ++i) + { + btVector3 vertices[3]; + for (auto j = 0u; j < 3; ++j) + { + const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; + vertices[j] = btVector3(vertex.x, vertex.y, vertex.z); + } + pack.triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + auto gimpactMeshShape = new btGImpactMeshShape(pack.triMesh.get()); + gimpactMeshShape->updateBound(); + + pack.collisionShape.reset(gimpactMeshShape); + + return pack; +} + +//============================================================================== +BulletCollisionDetector::BulletCollsionPack +BulletCollisionDetector::createBulletCollisionShape( + const dynamics::ShapePtr& shape) +{ + using dynamics::Shape; + using dynamics::BoxShape; + using dynamics::EllipsoidShape; + using dynamics::CylinderShape; + using dynamics::PlaneShape; + using dynamics::MeshShape; + using dynamics::SoftMeshShape; + + BulletCollsionPack pack; + + auto& bulletCollisionShape = pack.collisionShape; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + + const auto box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); + + bulletCollisionShape.reset(new btBoxShape(convertVector3(size*0.5))); + + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + + const auto ellipsoid = static_cast(shape.get()); + const Eigen::Vector3d& size = ellipsoid->getSize(); + + if (ellipsoid->isSphere()) + { + bulletCollisionShape.reset(new btSphereShape(size[0] * 0.5)); + } + else + { + // TODO(JS): Add mesh for ellipsoid + } + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + + const auto cylinder = static_cast(shape.get()); + const auto radius = cylinder->getRadius(); + const auto height = cylinder->getHeight(); + const auto size = btVector3(radius, radius, height * 0.5); + + bulletCollisionShape.reset(new btCylinderShapeZ(size)); + + break; + } + case Shape::PLANE: + { + assert(dynamic_cast(shape.get())); + + const auto plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + + bulletCollisionShape.reset(new btStaticPlaneShape( + convertVector3(normal), offset)); + + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + + const auto shapeMesh = static_cast(shape.get()); + const auto scale = shapeMesh->getScale(); + const auto mesh = shapeMesh->getMesh(); + + pack = createMesh(scale, mesh); + + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + + const auto softMeshShape = static_cast(shape.get()); + const auto mesh = softMeshShape->getAssimpMesh(); + + pack = createSoftMesh(mesh); + + break; + } + default: + { + dterr << "[BulletCollisionObjectData::init] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + + bulletCollisionShape.reset(new btSphereShape(0.1)); + + break; + } + } + + return pack; +} namespace { +//============================================================================== +BulletContactResultCallback::BulletContactResultCallback(Result& result) + : ContactResultCallback(), + mResult(result) +{ + // Do nothing +} + +//============================================================================== +//bool BulletContactResultCallback::needsCollision( +// btBroadphaseProxy* proxy0) const +//{ +// return true; +//} + +//============================================================================== btScalar BulletContactResultCallback::addSingleResult( btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index b5c69adff1de5..c1ccfdbaaabeb 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -38,6 +38,7 @@ #define DART_COLLISION_BULLET_BULLETCOLLISIONDETECTOR_H_ #include +#include #include #include "dart/collision/CollisionDetector.h" @@ -54,10 +55,13 @@ class BulletCollisionDetector : public CollisionDetector static std::shared_ptr create(); + /// Constructor + virtual ~BulletCollisionDetector(); + /// Return engine type "Bullet" static const std::string& getTypeStatic(); - // Documentation inherit + // Documentation inherited const std::string& getType() const override; using CollisionDetector::detect; @@ -67,34 +71,58 @@ class BulletCollisionDetector : public CollisionDetector /// Constructor BulletCollisionDetector() = default; - // Documentation inherit + // Documentation inherited std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; - // Documentation inherit + // Documentation inherited + void reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) override; + + // Documentation inherited std::unique_ptr createCollisionGroupData( CollisionGroup* parent, const CollisionObjectPtrs& collObjects) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object1, CollisionObjectData* object2, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; protected: + struct BulletCollsionPack + { + std::unique_ptr collisionShape; + std::unique_ptr triMesh; + }; + + BulletCollsionPack createMesh( + const Eigen::Vector3d& scale, const aiScene* mesh); + + BulletCollsionPack createSoftMesh(const aiMesh* mesh); + + BulletCollsionPack createBulletCollisionShape( + const dynamics::ShapePtr& shape); + +protected: + + using ShapeMapValue = std::pair; + + std::map mShapeMap; + std::shared_ptr mBulletCollisionGroupForSinglePair; }; diff --git a/dart/collision/bullet/BulletCollisionGroupData.cpp b/dart/collision/bullet/BulletCollisionGroupData.cpp index 90507cb0ab477..e110fd91f5803 100644 --- a/dart/collision/bullet/BulletCollisionGroupData.cpp +++ b/dart/collision/bullet/BulletCollisionGroupData.cpp @@ -166,17 +166,17 @@ bool BulletCollisionGroupData::CollisionFilter::needBroadphaseCollision( if (collide) { - auto collObj0 = static_cast(proxy0->m_clientObject); - auto collObj1 = static_cast(proxy1->m_clientObject); +// auto collObj0 = static_cast(proxy0->m_clientObject); +// auto collObj1 = static_cast(proxy1->m_clientObject); - auto userData0 = static_cast(collObj0->getUserPointer()); - auto userData1 = static_cast(collObj1->getUserPointer()); +// auto userData0 = static_cast(collObj0->getUserPointer()); +// auto userData1 = static_cast(collObj1->getUserPointer()); - // Assume single collision detector - assert(userData0->collisionDetector == userData1->collisionDetector); +// // Assume single collision detector +// assert(userData0->collisionDetector == userData1->collisionDetector); - auto collGroup0 = userData0->group; - auto collGroup1 = userData1->group; +// auto collGroup0 = userData0->group; +// auto collGroup1 = userData1->group; // if (!collGroup0 || !) diff --git a/dart/collision/bullet/BulletCollisionGroupData.h b/dart/collision/bullet/BulletCollisionGroupData.h index 80acd79b3735b..9906b91bf00f9 100644 --- a/dart/collision/bullet/BulletCollisionGroupData.h +++ b/dart/collision/bullet/BulletCollisionGroupData.h @@ -102,7 +102,7 @@ class BulletCollisionGroupData : public CollisionGroupData std::unique_ptr mBulletProadphaseAlg; /// Bullet collision filter - std::unique_ptr mBulletCollisionFilter; + std::unique_ptr mBulletDefaultCollisionFilter; /// Bullet collision configuration std::unique_ptr mBulletCollisionConfiguration; diff --git a/dart/collision/bullet/BulletCollisionObjectData.cpp b/dart/collision/bullet/BulletCollisionObjectData.cpp index 68938aa963117..38335fa1fe9fd 100644 --- a/dart/collision/bullet/BulletCollisionObjectData.cpp +++ b/dart/collision/bullet/BulletCollisionObjectData.cpp @@ -42,35 +42,21 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/bullet/BulletTypes.h" #include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { //============================================================================== -BulletCollisionObjectUserData::BulletCollisionObjectUserData() - : shape(nullptr), - collisionObject(nullptr), - collisionDetector(nullptr), - group(nullptr) -{ - // Do nothing -} - -//============================================================================== -BulletCollisionObjectData::BulletCollisionObjectData( +BulletCollisionObjectUserData::BulletCollisionObjectUserData( + CollisionObject* collisionObject, CollisionDetector* collisionDetector, - CollisionObject* parent, - const dynamics::ShapePtr& shape) - : CollisionObjectData(collisionDetector, parent) + CollisionGroup* collisionGroup) + : collisionObject(collisionObject), + collisionDetector(collisionDetector), + group(collisionGroup) { - updateShape(shape); + // Do nothing } //============================================================================== @@ -79,125 +65,6 @@ void BulletCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) mBulletCollisionObject->setWorldTransform(convertTransform(tf)); } -//============================================================================== -void BulletCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) -{ - mBulletTriangleMesh.release(); - mBulletGImpactMeshShape.release(); - mBulletCollisionShape.release(); - mBulletCollisionObject.release(); - - using dynamics::Shape; - using dynamics::BoxShape; - using dynamics::EllipsoidShape; - using dynamics::CylinderShape; - using dynamics::PlaneShape; - using dynamics::MeshShape; - using dynamics::SoftMeshShape; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - - const auto box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); - - mBulletCollisionShape.reset(new btBoxShape(convertVector3(size*0.5))); - - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - - const auto ellipsoid = static_cast(shape.get()); - const Eigen::Vector3d& size = ellipsoid->getSize(); - - if (ellipsoid->isSphere()) - { - mBulletCollisionShape.reset(new btSphereShape(size[0] * 0.5)); - } - else - { - // TODO(JS): Add mesh for ellipsoid - } - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - - const auto cylinder = static_cast(shape.get()); - const auto radius = cylinder->getRadius(); - const auto height = cylinder->getHeight(); - const auto size = btVector3(radius, radius, height * 0.5); - - mBulletCollisionShape.reset(new btCylinderShapeZ(size)); - - break; - } - case Shape::PLANE: - { - assert(dynamic_cast(shape.get())); - - const auto plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - - mBulletCollisionShape.reset(new btStaticPlaneShape( - convertVector3(normal), offset)); - - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - - const auto shapeMesh = static_cast(shape.get()); - const auto scale = shapeMesh->getScale(); - const auto mesh = shapeMesh->getMesh(); - - mBulletCollisionShape.reset(createMesh(scale, mesh)); - - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - - const auto softMeshShape = static_cast(shape.get()); - const auto mesh = softMeshShape->getAssimpMesh(); - - mBulletCollisionShape.reset(createSoftMesh(mesh)); - - break; - } - default: - { - dterr << "[BulletCollisionObjectData::init] " - << "Attempting to create unsupported shape type '" - << shape->getShapeType() << "'.\n"; - - mBulletCollisionShape.reset(new btSphereShape(0.1)); - - break; - } - } - - mBulletCollisionObjectUserData.reset(new BulletCollisionObjectUserData()); - mBulletCollisionObjectUserData->shape = shape; - mBulletCollisionObjectUserData->collisionObject = mParent; - mBulletCollisionObjectUserData->collisionDetector = mCollisionDetector; - mBulletCollisionObjectUserData->group = nullptr; - - mBulletCollisionObject.reset(new btCollisionObject()); - mBulletCollisionObject->setCollisionShape(mBulletCollisionShape.get()); - mBulletCollisionObject->setUserPointer(mBulletCollisionObjectUserData.get()); -} - //============================================================================== void BulletCollisionObjectData::update() { @@ -211,57 +78,17 @@ btCollisionObject* BulletCollisionObjectData::getBulletCollisionObject() const } //============================================================================== -btGImpactMeshShape* BulletCollisionObjectData::createMesh( - const Eigen::Vector3d& scale, const aiScene* mesh) -{ - mBulletTriangleMesh.reset(new btTriangleMesh()); - - for (unsigned int i = 0; i < mesh->mNumMeshes; i++) - { - for (unsigned int j = 0; j < mesh->mMeshes[i]->mNumFaces; j++) - { - btVector3 vertices[3]; - for (unsigned int k = 0; k < 3; k++) - { - const aiVector3D& vertex = mesh->mMeshes[i]->mVertices[ - mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = btVector3(vertex.x * scale[0], - vertex.y * scale[1], - vertex.z * scale[2]); - } - mBulletTriangleMesh->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - - btGImpactMeshShape* gimpactMeshShape - = new btGImpactMeshShape(mBulletTriangleMesh.get()); - gimpactMeshShape->updateBound(); - - return gimpactMeshShape; -} - -//============================================================================== -btGImpactMeshShape* BulletCollisionObjectData::createSoftMesh( - const aiMesh* mesh) +BulletCollisionObjectData::BulletCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent, + btCollisionShape* bulletCollisionShape) + : CollisionObjectData(collisionDetector, parent), + mBulletCollisionObjectUserData(new BulletCollisionObjectUserData( + mParent, mCollisionDetector, nullptr)), + mBulletCollisionObject(new btCollisionObject()) { - mBulletTriangleMesh.reset(new btTriangleMesh()); - - for (auto i = 0u; i < mesh->mNumFaces; ++i) - { - btVector3 vertices[3]; - for (auto j = 0u; j < 3; ++j) - { - const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; - vertices[j] = btVector3(vertex.x, vertex.y, vertex.z); - } - mBulletTriangleMesh->addTriangle(vertices[0], vertices[1], vertices[2]); - } - - btGImpactMeshShape* gimpactMeshShape - = new btGImpactMeshShape(mBulletTriangleMesh.get()); - gimpactMeshShape->updateBound(); - - return gimpactMeshShape; + mBulletCollisionObject->setCollisionShape(bulletCollisionShape); + mBulletCollisionObject->setUserPointer(mBulletCollisionObjectUserData.get()); } } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionObjectData.h b/dart/collision/bullet/BulletCollisionObjectData.h index 6d4e48515506b..f650a0019028a 100644 --- a/dart/collision/bullet/BulletCollisionObjectData.h +++ b/dart/collision/bullet/BulletCollisionObjectData.h @@ -52,30 +52,25 @@ class CollisionObject; struct BulletCollisionObjectUserData { - dynamics::ConstShapePtr shape; CollisionObject* collisionObject; CollisionDetector* collisionDetector; CollisionGroup* group; - BulletCollisionObjectUserData(); + BulletCollisionObjectUserData(CollisionObject* collisionObject, + CollisionDetector* collisionDetector, + CollisionGroup* collisionGroup); }; class BulletCollisionObjectData : public CollisionObjectData { public: - /// Constructor - BulletCollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent, - const dynamics::ShapePtr& shape); + friend class BulletCollisionDetector; // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; - // Documentation inherited - void updateShape(const dynamics::ShapePtr& shape) override; - // Documentation inherited void update() override; @@ -84,23 +79,16 @@ class BulletCollisionObjectData : public CollisionObjectData protected: - btGImpactMeshShape* createMesh(const Eigen::Vector3d& scale, - const aiScene* mesh); + /// Constructor + BulletCollisionObjectData(CollisionDetector* collisionDetector, + CollisionObject* parent, + btCollisionShape* bulletCollisionShape); - btGImpactMeshShape* createSoftMesh(const aiMesh* mesh); +protected: /// Bullet collision geometry user data std::unique_ptr mBulletCollisionObjectUserData; - /// Bullet triangle mesh - std::unique_ptr mBulletTriangleMesh; - - /// Bullet GImpact mesh - std::unique_ptr mBulletGImpactMeshShape; - - /// Bullet collision geometry user data - std::unique_ptr mBulletCollisionShape; - /// Bullet collision object std::unique_ptr mBulletCollisionObject; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 33aca6635a234..73a34385f6564 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -89,6 +89,13 @@ DARTCollisionDetector::createCollisionObjectData( new DARTCollisionObjectData(this, parent)); } +//============================================================================== +void DARTCollisionDetector::reclaimCollisionObjectData( + CollisionObjectData* /*collisionObjectData*/) +{ + // Do nothing +} + //============================================================================== std::unique_ptr DARTCollisionDetector::createCollisionGroupData( @@ -107,8 +114,10 @@ bool DARTCollisionDetector::detect( { result.contacts.clear(); - assert(objectData1->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); - assert(objectData2->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(objectData1->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); + assert(objectData2->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); auto collObj1 = objectData1->getCollisionObject(); auto collObj2 = objectData2->getCollisionObject(); @@ -126,8 +135,10 @@ bool DARTCollisionDetector::detect( assert(objectData); assert(groupData); - assert(objectData->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); - assert(groupData->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(objectData->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); auto collObj1 = objectData->getCollisionObject(); auto collGrp = groupData->getCollisionGroup(); @@ -147,7 +158,8 @@ bool DARTCollisionDetector::detect( result.contacts.clear(); assert(groupData); - assert(groupData->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); auto collGrp = groupData->getCollisionGroup(); auto collObjs = collGrp->getCollisionObjects(); @@ -180,8 +192,10 @@ bool DARTCollisionDetector::detect( assert(groupData1); assert(groupData2); - assert(groupData1->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); - assert(groupData2->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); + assert(groupData1->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); + assert(groupData2->getCollisionDetector()->getType() + == DARTCollisionDetector::getTypeStatic()); auto collGrp1 = groupData1->getCollisionGroup(); auto collGrp2 = groupData2->getCollisionGroup(); diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index a059aefa4a6d6..4b15a8d4b2784 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -53,7 +53,7 @@ class DARTCollisionDetector : public CollisionDetector /// Return engine type "FCLMesh" static const std::string& getTypeStatic(); - // Documentation inherit + // Documentation inherited const std::string& getType() const override; using CollisionDetector::detect; @@ -63,29 +63,33 @@ class DARTCollisionDetector : public CollisionDetector /// Constructor DARTCollisionDetector() = default; - // Documentation inherit + // Documentation inherited std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; - // Documentation inherit + // Documentation inherited + void reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) override; + + // Documentation inherited std::unique_ptr createCollisionGroupData( CollisionGroup* parent, const CollisionObjectPtrs& collObjects) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object1, CollisionObjectData* object2, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; diff --git a/dart/collision/dart/DARTCollisionObjectData.cpp b/dart/collision/dart/DARTCollisionObjectData.cpp index 48796b5a6f40b..dafa06cdabc88 100644 --- a/dart/collision/dart/DARTCollisionObjectData.cpp +++ b/dart/collision/dart/DARTCollisionObjectData.cpp @@ -39,15 +39,6 @@ namespace dart { namespace collision { -//============================================================================== -DARTCollisionObjectData::DARTCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent) - : CollisionObjectData(collisionDetector, parent) -{ - // Do nothing -} - //============================================================================== void DARTCollisionObjectData::updateTransform(const Eigen::Isometry3d& /*tf*/) { @@ -55,13 +46,16 @@ void DARTCollisionObjectData::updateTransform(const Eigen::Isometry3d& /*tf*/) } //============================================================================== -void DARTCollisionObjectData::updateShape(const dynamics::ShapePtr& /*shape*/) +void DARTCollisionObjectData::update() { // Do nothing } //============================================================================== -void DARTCollisionObjectData::update() +DARTCollisionObjectData::DARTCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent) + : CollisionObjectData(collisionDetector, parent) { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionObjectData.h b/dart/collision/dart/DARTCollisionObjectData.h index ca39102ff5364..eea029059d315 100644 --- a/dart/collision/dart/DARTCollisionObjectData.h +++ b/dart/collision/dart/DARTCollisionObjectData.h @@ -49,20 +49,20 @@ class DARTCollisionObjectData : public CollisionObjectData { public: - /// Constructor - DARTCollisionObjectData(CollisionDetector* collisionDetector, CollisionObject* parent); + friend class DARTCollisionDetector; // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; - // Documentation inherited - void updateShape(const dynamics::ShapePtr& shape) override; - // Documentation inherited void update() override; protected: + /// Constructor + DARTCollisionObjectData(CollisionDetector* collisionDetector, + CollisionObject* parent); + }; } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 4d06596c80a02..590938528c2f6 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -37,22 +37,36 @@ #include "dart/collision/fcl/FCLCollisionDetector.h" +#include + #include #include #include +#include #include +#include +#include +#include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" #include "dart/collision/fcl/FCLCollisionGroupData.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { namespace { -bool checkPair(fcl::CollisionObject* o1, +bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); @@ -69,8 +83,13 @@ Contact convertContact(const fcl::Contact& fclContact, /// Collision data stores the collision request and the result given by /// collision algorithm. -struct FCLCollisionData +struct FCLCollisionCallbackData { + CollisionGroup* mCollisionGroup; + + /// Collision filter + CollisionFilter* mFilter; + /// FCL collision request fcl::CollisionRequest mFclRequest; @@ -87,10 +106,14 @@ struct FCLCollisionData bool done; /// Constructor - FCLCollisionData( + FCLCollisionCallbackData( +// CollisionGroup* collisionGroup, +// CollisionFilter* filter, const Option* option = nullptr, Result* result = nullptr) - : mOption(option), + : /*mCollisionGroup(collisionGroup), + mFilter(filter),*/ + mOption(option), mResult(result), done(false) { @@ -99,6 +122,541 @@ struct FCLCollisionData } }; +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + +//============================================================================== +// Create a cube mesh for collision detection +template +fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) +{ + int faces[6][4] = + { + {0, 1, 2, 3}, + {3, 2, 6, 7}, + {7, 6, 5, 4}, + {4, 5, 1, 0}, + {5, 6, 2, 1}, + {7, 4, 0, 3} + }; + float v[8][3]; + + v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; + v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; + v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; + v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; + v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; + v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 6; i++) + { + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + model->addTriangle(p1, p2, p3); + + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + model->addTriangle(p1, p2, p3); + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) +{ + float v[59][3] = + { + {0, 0, 0}, + {0.135299, -0.461940, -0.135299}, + {0.000000, -0.461940, -0.191342}, + {-0.135299, -0.461940, -0.135299}, + {-0.191342, -0.461940, 0.000000}, + {-0.135299, -0.461940, 0.135299}, + {0.000000, -0.461940, 0.191342}, + {0.135299, -0.461940, 0.135299}, + {0.191342, -0.461940, 0.000000}, + {0.250000, -0.353553, -0.250000}, + {0.000000, -0.353553, -0.353553}, + {-0.250000, -0.353553, -0.250000}, + {-0.353553, -0.353553, 0.000000}, + {-0.250000, -0.353553, 0.250000}, + {0.000000, -0.353553, 0.353553}, + {0.250000, -0.353553, 0.250000}, + {0.353553, -0.353553, 0.000000}, + {0.326641, -0.191342, -0.326641}, + {0.000000, -0.191342, -0.461940}, + {-0.326641, -0.191342, -0.326641}, + {-0.461940, -0.191342, 0.000000}, + {-0.326641, -0.191342, 0.326641}, + {0.000000, -0.191342, 0.461940}, + {0.326641, -0.191342, 0.326641}, + {0.461940, -0.191342, 0.000000}, + {0.353553, 0.000000, -0.353553}, + {0.000000, 0.000000, -0.500000}, + {-0.353553, 0.000000, -0.353553}, + {-0.500000, 0.000000, 0.000000}, + {-0.353553, 0.000000, 0.353553}, + {0.000000, 0.000000, 0.500000}, + {0.353553, 0.000000, 0.353553}, + {0.500000, 0.000000, 0.000000}, + {0.326641, 0.191342, -0.326641}, + {0.000000, 0.191342, -0.461940}, + {-0.326641, 0.191342, -0.326641}, + {-0.461940, 0.191342, 0.000000}, + {-0.326641, 0.191342, 0.326641}, + {0.000000, 0.191342, 0.461940}, + {0.326641, 0.191342, 0.326641}, + {0.461940, 0.191342, 0.000000}, + {0.250000, 0.353553, -0.250000}, + {0.000000, 0.353553, -0.353553}, + {-0.250000, 0.353553, -0.250000}, + {-0.353553, 0.353553, 0.000000}, + {-0.250000, 0.353553, 0.250000}, + {0.000000, 0.353553, 0.353553}, + {0.250000, 0.353553, 0.250000}, + {0.353553, 0.353553, 0.000000}, + {0.135299, 0.461940, -0.135299}, + {0.000000, 0.461940, -0.191342}, + {-0.135299, 0.461940, -0.135299}, + {-0.191342, 0.461940, 0.000000}, + {-0.135299, 0.461940, 0.135299}, + {0.000000, 0.461940, 0.191342}, + {0.135299, 0.461940, 0.135299}, + {0.191342, 0.461940, 0.000000}, + {0.000000, -0.500000, 0.000000}, + {0.000000, 0.500000, 0.000000} + }; + + int f[112][3] = + { + {1, 2, 9}, + {9, 2, 10}, + {2, 3, 10}, + {10, 3, 11}, + {3, 4, 11}, + {11, 4, 12}, + {4, 5, 12}, + {12, 5, 13}, + {5, 6, 13}, + {13, 6, 14}, + {6, 7, 14}, + {14, 7, 15}, + {7, 8, 15}, + {15, 8, 16}, + {8, 1, 16}, + {16, 1, 9}, + {9, 10, 17}, + {17, 10, 18}, + {10, 11, 18}, + {18, 11, 19}, + {11, 12, 19}, + {19, 12, 20}, + {12, 13, 20}, + {20, 13, 21}, + {13, 14, 21}, + {21, 14, 22}, + {14, 15, 22}, + {22, 15, 23}, + {15, 16, 23}, + {23, 16, 24}, + {16, 9, 24}, + {24, 9, 17}, + {17, 18, 25}, + {25, 18, 26}, + {18, 19, 26}, + {26, 19, 27}, + {19, 20, 27}, + {27, 20, 28}, + {20, 21, 28}, + {28, 21, 29}, + {21, 22, 29}, + {29, 22, 30}, + {22, 23, 30}, + {30, 23, 31}, + {23, 24, 31}, + {31, 24, 32}, + {24, 17, 32}, + {32, 17, 25}, + {25, 26, 33}, + {33, 26, 34}, + {26, 27, 34}, + {34, 27, 35}, + {27, 28, 35}, + {35, 28, 36}, + {28, 29, 36}, + {36, 29, 37}, + {29, 30, 37}, + {37, 30, 38}, + {30, 31, 38}, + {38, 31, 39}, + {31, 32, 39}, + {39, 32, 40}, + {32, 25, 40}, + {40, 25, 33}, + {33, 34, 41}, + {41, 34, 42}, + {34, 35, 42}, + {42, 35, 43}, + {35, 36, 43}, + {43, 36, 44}, + {36, 37, 44}, + {44, 37, 45}, + {37, 38, 45}, + {45, 38, 46}, + {38, 39, 46}, + {46, 39, 47}, + {39, 40, 47}, + {47, 40, 48}, + {40, 33, 48}, + {48, 33, 41}, + {41, 42, 49}, + {49, 42, 50}, + {42, 43, 50}, + {50, 43, 51}, + {43, 44, 51}, + {51, 44, 52}, + {44, 45, 52}, + {52, 45, 53}, + {45, 46, 53}, + {53, 46, 54}, + {46, 47, 54}, + {54, 47, 55}, + {47, 48, 55}, + {55, 48, 56}, + {48, 41, 56}, + {56, 41, 49}, + {2, 1, 57}, + {3, 2, 57}, + {4, 3, 57}, + {5, 4, 57}, + {6, 5, 57}, + {7, 6, 57}, + {8, 7, 57}, + {1, 8, 57}, + {49, 50, 58}, + {50, 51, 58}, + {51, 52, 58}, + {52, 53, 58}, + {53, 54, 58}, + {54, 55, 58}, + {55, 56, 58}, + {56, 49, 58} + }; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 112; i++) + { + p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, + v[f[i][0]][1] * _sizeY, + v[f[i][0]][2] * _sizeZ); + p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, + v[f[i][1]][1] * _sizeY, + v[f[i][1]][2] * _sizeZ); + p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, + v[f[i][2]][1] * _sizeY, + v[f[i][2]][2] * _sizeZ); + + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + + return model; +} + +#endif + +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) + +//============================================================================== +template +fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, + double _height, int _slices, int _stacks) +{ + const int CACHE_SIZE = 240; + + int i, j; + float sinCache[CACHE_SIZE]; + float cosCache[CACHE_SIZE]; + float angle; + float zBase; + float zLow, zHigh; + float sintemp, costemp; + float deltaRadius; + float radiusLow, radiusHigh; + + if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; + + if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || + _height < 0.0) + { + return nullptr; + } + + /* Center at CoM */ + zBase = -_height/2; + + /* Compute delta */ + deltaRadius = _baseRadius - _topRadius; + + /* Cache is the vertex locations cache */ + for (i = 0; i < _slices; i++) + { + angle = 2 * M_PI * i / _slices; + sinCache[i] = sin(angle); + cosCache[i] = cos(angle); + } + + sinCache[_slices] = sinCache[0]; + cosCache[_slices] = cosCache[0]; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3, p4; + + model->beginModel(); + + /* Base of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _baseRadius; + zLow = zBase; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + /* Body of cylinder */ + for (i = 0; i < _slices; i++) + { + for (j = 0; j < _stacks; j++) + { + zLow = j * _height / _stacks + zBase; + zHigh = (j + 1) * _height / _stacks + zBase; + radiusLow = _baseRadius + - deltaRadius * (static_cast(j) / _stacks); + radiusHigh = _baseRadius + - deltaRadius * (static_cast(j + 1) / _stacks); + + p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); + + model->addTriangle(p1, p2, p3); + model->addTriangle(p2, p3, p4); + } + } + + /* Top of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _topRadius; + zLow = zBase + _height; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + return model; +} + +#endif + +//============================================================================== +template +fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, + const aiScene* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + for (size_t i = 0; i < _mesh->mNumMeshes; i++) + { + for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) + { + fcl::Vec3f vertices[3]; + for (size_t k = 0; k < 3; k++) + { + const aiVector3D& vertex + = _mesh->mMeshes[i]->mVertices[ + _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + vertices[k] = fcl::Vec3f(vertex.x * _scaleX, + vertex.y * _scaleY, + vertex.z * _scaleZ); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + + for (size_t i = 0; i < _mesh->mNumFaces; i++) + { + fcl::Vec3f vertices[3]; + for (size_t j = 0; j < 3; j++) + { + const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; + vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + model->endModel(); + return model; +} + +//============================================================================== +boost::shared_ptr createFCLCollisionGeometry( + const dynamics::ShapePtr& shape) +{ + using dynamics::Shape; + using dynamics::BoxShape; + using dynamics::EllipsoidShape; + using dynamics::CylinderShape; + using dynamics::PlaneShape; + using dynamics::MeshShape; + using dynamics::SoftMeshShape; + + boost::shared_ptr fclCollGeom; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + const BoxShape* box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(size[0], size[1], size[2])); +#else + fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); +#endif + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + EllipsoidShape* ellipsoid = static_cast(shape.get()); + + const Eigen::Vector3d& size = ellipsoid->getSize(); + + if (ellipsoid->isSphere()) + { + fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); + } + else + { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset( + createEllipsoid(ellipsoid->getSize()[0], + ellipsoid->getSize()[1], + ellipsoid->getSize()[2])); +#else + fclCollGeom.reset( + new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); +#endif + } + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + const CylinderShape* cylinder + = static_cast(shape.get()); + const double radius = cylinder->getRadius(); + const double height = cylinder->getHeight(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); +#else + fclCollGeom.reset(new fcl::Cylinder(radius, height)); +#endif + // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 + // returns single contact point for cylinder yet. + break; + } + case Shape::PLANE: + { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); +#else + assert(dynamic_cast(shape.get())); + dynamics::PlaneShape* plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + fclCollGeom.reset( + new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); +#endif + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + MeshShape* shapeMesh = static_cast(shape.get()); + fclCollGeom.reset( + createMesh(shapeMesh->getScale()[0], + shapeMesh->getScale()[1], + shapeMesh->getScale()[2], + shapeMesh->getMesh())); + + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + SoftMeshShape* softMeshShape = static_cast(shape.get()); + fclCollGeom.reset( + createSoftMesh(softMeshShape->getAssimpMesh())); + + break; + } + default: + { + dterr << "[FCLCollisionObjectData::updateShape] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + return nullptr; + } + } + + return fclCollGeom; +} + } // anonymous namespace @@ -109,6 +667,12 @@ std::shared_ptr FCLCollisionDetector::create() return std::shared_ptr(new FCLCollisionDetector()); } +//============================================================================== +FCLCollisionDetector::~FCLCollisionDetector() +{ + assert(mShapeMap.empty()); +} + //============================================================================== const std::string& FCLCollisionDetector::getTypeStatic() { @@ -127,8 +691,41 @@ std::unique_ptr FCLCollisionDetector::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { + boost::shared_ptr fclCollGeom; + + auto findResult = mShapeMap.find(shape); + if (mShapeMap.end() != findResult) + { + fclCollGeom = findResult->second.first; + } + else + { + fclCollGeom = createFCLCollisionGeometry(shape); + mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); + } + return std::unique_ptr( - new FCLCollisionObjectData(this, parent, shape)); + new FCLCollisionObjectData(this, parent, fclCollGeom)); +} + +//============================================================================== +void FCLCollisionDetector::reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) +{ + // Retrieve associated shape + auto shape = collisionObjectData->getCollisionObject()->getShape(); + assert(shape); + + auto findResult = mShapeMap.find(shape); + assert(mShapeMap.end() != findResult); + + auto fclCollGeomAndCount = findResult->second; + assert(0u != fclCollGeomAndCount.second); + + fclCollGeomAndCount.second--; + + if (0u == fclCollGeomAndCount.second) + mShapeMap.erase(findResult); } //============================================================================== @@ -149,8 +746,10 @@ bool FCLCollisionDetector::detect( { result.contacts.clear(); - assert(objectData1->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); - assert(objectData2->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(objectData1->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); + assert(objectData2->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); auto castedData1 = static_cast(objectData1); auto castedData2 = static_cast(objectData2); @@ -158,8 +757,8 @@ bool FCLCollisionDetector::detect( auto fclCollObj1 = castedData1->getFCLCollisionObject(); auto fclCollObj2 = castedData2->getFCLCollisionObject(); - FCLCollisionData collData(&option, &result); - checkPair(fclCollObj1, fclCollObj2, &collData); + FCLCollisionCallbackData collData(&option, &result); + collisionCallback(fclCollObj1, fclCollObj2, &collData); return !result.contacts.empty(); } @@ -174,8 +773,10 @@ bool FCLCollisionDetector::detect( assert(objectData); assert(groupData); - assert(objectData->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); - assert(groupData->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(objectData->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); auto castedObjData = static_cast(objectData); auto castedGrpData = static_cast(groupData); @@ -183,8 +784,8 @@ bool FCLCollisionDetector::detect( auto fclObject = castedObjData->getFCLCollisionObject(); auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); - FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(fclObject, &collData, checkPair); + FCLCollisionCallbackData collData(&option, &result); + broadPhaseAlg->collide(fclObject, &collData, collisionCallback); return !result.contacts.empty(); } @@ -197,14 +798,15 @@ bool FCLCollisionDetector::detect( result.contacts.clear(); assert(groupData); - assert(groupData->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(groupData->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); auto castedData = static_cast(groupData); auto broadPhaseAlg = castedData->getFCLCollisionManager(); - FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(&collData, checkPair); + FCLCollisionCallbackData collData(&option, &result); + broadPhaseAlg->collide(&collData, collisionCallback); return !result.contacts.empty(); } @@ -219,8 +821,10 @@ bool FCLCollisionDetector::detect( assert(groupData1); assert(groupData2); - assert(groupData1->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); - assert(groupData2->getCollisionDetector()->getType() == FCLCollisionDetector::getTypeStatic()); + assert(groupData1->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); + assert(groupData2->getCollisionDetector()->getType() + == FCLCollisionDetector::getTypeStatic()); auto castedData1 = static_cast(groupData1); auto castedData2 = static_cast(groupData2); @@ -228,34 +832,50 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); - FCLCollisionData collData(&option, &result); - broadPhaseAlg1->collide(broadPhaseAlg2, &collData, checkPair); + FCLCollisionCallbackData collData(&option, &result); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); return !result.contacts.empty(); } +//============================================================================== +FCLCollisionObject* FCLCollisionDetector::findCollisionObject( + fcl::CollisionObject* /*fclCollObj*/) +{ + return nullptr; +} + namespace { //============================================================================== -bool checkPair(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) +bool collisionCallback( + fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { - FCLCollisionData* collData = static_cast(cdata); + auto collData = static_cast(cdata); - const fcl::CollisionRequest& fclRequest = collData->mFclRequest; - fcl::CollisionResult& fclResult = collData->mFclResult; - Result& result = *(collData->mResult); -// FCLEngine* cd = collData->collisionDetector; - // TODO(JS): take filter object instead of collision detector + const auto& fclRequest = collData->mFclRequest; + auto& fclResult = collData->mFclResult; + auto& result = *(collData->mResult); + auto filter = collData->mFilter; if (collData->done) return true; // Filtering -// if (!cd->isCollidable(cd->findCollisionNode(o1), cd->findCollisionNode(o2))) -// return collData->done; - // TODO(JS): disabled until other functionalities are implemented + if (filter) + { +// auto collisionGroup = collData->mCollisionGroup; +// auto collisionGroupData +// = static_cast(collisionGroup->getEngineData()); + +// auto collObj1 = collisionGroupData->findCollisionObject(o1); +// auto collObj2 = collisionGroupData->findCollisionObject(o2); + +// if (filter->needCollision(collObj1, collObj2)) +// return collData->done; + } // Clear previous results fclResult.clear(); @@ -382,10 +1002,8 @@ Contact convertContact(const fcl::Contact& fclContact, contact.triID1 = fclContact.b1; contact.triID2 = fclContact.b2; - FCLCollisionObjectUserData* userData1 - = static_cast(o1->getUserData()); - FCLCollisionObjectUserData* userData2 - = static_cast(o2->getUserData()); + auto userData1 = static_cast(o1->getUserData()); + auto userData2 = static_cast(o2->getUserData()); assert(userData1); assert(userData2); contact.collisionObject1 = userData1->mCollisionObject; diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index eb48f546c1f04..8fca7e3df359c 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -39,22 +39,27 @@ #define DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_H_ #include - +#include #include "dart/collision/CollisionDetector.h" namespace dart { namespace collision { +class FCLCollisionObject; + class FCLCollisionDetector : public CollisionDetector { public: static std::shared_ptr create(); + /// Constructor + virtual ~FCLCollisionDetector(); + /// Return engine type "FCL" static const std::string& getTypeStatic(); - // Documentation inherit + // Documentation inherited const std::string& getType() const override; using CollisionDetector::detect; @@ -64,32 +69,47 @@ class FCLCollisionDetector : public CollisionDetector /// Constructor FCLCollisionDetector() = default; - // Documentation inherit + // Documentation inherited std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; - // Documentation inherit + // Documentation inherited + void reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) override; + + // Documentation inherited std::unique_ptr createCollisionGroupData( CollisionGroup* parent, const CollisionObjectPtrs& collObjects) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object1, CollisionObjectData* object2, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; + FCLCollisionObject* findCollisionObject(fcl::CollisionObject* fclCollObj); + +protected: + + using ShapeMapValue + = std::pair, size_t>; + + std::map mShapeMap; + + std::map mFCLObjectMap; + }; } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionObjectData.cpp b/dart/collision/fcl/FCLCollisionObjectData.cpp index 5ff32b1f36270..9714c34c1d5a6 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLCollisionObjectData.cpp @@ -36,590 +36,25 @@ #include "dart/collision/fcl/FCLCollisionObjectData.h" -#include #include -#include -#include "dart/common/Console.h" #include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { -namespace { - -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - -//============================================================================== -// Create a cube mesh for collision detection -template -fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) -{ - int faces[6][4] = - { - {0, 1, 2, 3}, - {3, 2, 6, 7}, - {7, 6, 5, 4}, - {4, 5, 1, 0}, - {5, 6, 2, 1}, - {7, 4, 0, 3} - }; - float v[8][3]; - - v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; - v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; - v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; - v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; - v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; - v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 6; i++) - { - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); - p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - model->addTriangle(p1, p2, p3); - - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); - model->addTriangle(p1, p2, p3); - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) -{ - float v[59][3] = - { - {0, 0, 0}, - {0.135299, -0.461940, -0.135299}, - {0.000000, -0.461940, -0.191342}, - {-0.135299, -0.461940, -0.135299}, - {-0.191342, -0.461940, 0.000000}, - {-0.135299, -0.461940, 0.135299}, - {0.000000, -0.461940, 0.191342}, - {0.135299, -0.461940, 0.135299}, - {0.191342, -0.461940, 0.000000}, - {0.250000, -0.353553, -0.250000}, - {0.000000, -0.353553, -0.353553}, - {-0.250000, -0.353553, -0.250000}, - {-0.353553, -0.353553, 0.000000}, - {-0.250000, -0.353553, 0.250000}, - {0.000000, -0.353553, 0.353553}, - {0.250000, -0.353553, 0.250000}, - {0.353553, -0.353553, 0.000000}, - {0.326641, -0.191342, -0.326641}, - {0.000000, -0.191342, -0.461940}, - {-0.326641, -0.191342, -0.326641}, - {-0.461940, -0.191342, 0.000000}, - {-0.326641, -0.191342, 0.326641}, - {0.000000, -0.191342, 0.461940}, - {0.326641, -0.191342, 0.326641}, - {0.461940, -0.191342, 0.000000}, - {0.353553, 0.000000, -0.353553}, - {0.000000, 0.000000, -0.500000}, - {-0.353553, 0.000000, -0.353553}, - {-0.500000, 0.000000, 0.000000}, - {-0.353553, 0.000000, 0.353553}, - {0.000000, 0.000000, 0.500000}, - {0.353553, 0.000000, 0.353553}, - {0.500000, 0.000000, 0.000000}, - {0.326641, 0.191342, -0.326641}, - {0.000000, 0.191342, -0.461940}, - {-0.326641, 0.191342, -0.326641}, - {-0.461940, 0.191342, 0.000000}, - {-0.326641, 0.191342, 0.326641}, - {0.000000, 0.191342, 0.461940}, - {0.326641, 0.191342, 0.326641}, - {0.461940, 0.191342, 0.000000}, - {0.250000, 0.353553, -0.250000}, - {0.000000, 0.353553, -0.353553}, - {-0.250000, 0.353553, -0.250000}, - {-0.353553, 0.353553, 0.000000}, - {-0.250000, 0.353553, 0.250000}, - {0.000000, 0.353553, 0.353553}, - {0.250000, 0.353553, 0.250000}, - {0.353553, 0.353553, 0.000000}, - {0.135299, 0.461940, -0.135299}, - {0.000000, 0.461940, -0.191342}, - {-0.135299, 0.461940, -0.135299}, - {-0.191342, 0.461940, 0.000000}, - {-0.135299, 0.461940, 0.135299}, - {0.000000, 0.461940, 0.191342}, - {0.135299, 0.461940, 0.135299}, - {0.191342, 0.461940, 0.000000}, - {0.000000, -0.500000, 0.000000}, - {0.000000, 0.500000, 0.000000} - }; - - int f[112][3] = - { - {1, 2, 9}, - {9, 2, 10}, - {2, 3, 10}, - {10, 3, 11}, - {3, 4, 11}, - {11, 4, 12}, - {4, 5, 12}, - {12, 5, 13}, - {5, 6, 13}, - {13, 6, 14}, - {6, 7, 14}, - {14, 7, 15}, - {7, 8, 15}, - {15, 8, 16}, - {8, 1, 16}, - {16, 1, 9}, - {9, 10, 17}, - {17, 10, 18}, - {10, 11, 18}, - {18, 11, 19}, - {11, 12, 19}, - {19, 12, 20}, - {12, 13, 20}, - {20, 13, 21}, - {13, 14, 21}, - {21, 14, 22}, - {14, 15, 22}, - {22, 15, 23}, - {15, 16, 23}, - {23, 16, 24}, - {16, 9, 24}, - {24, 9, 17}, - {17, 18, 25}, - {25, 18, 26}, - {18, 19, 26}, - {26, 19, 27}, - {19, 20, 27}, - {27, 20, 28}, - {20, 21, 28}, - {28, 21, 29}, - {21, 22, 29}, - {29, 22, 30}, - {22, 23, 30}, - {30, 23, 31}, - {23, 24, 31}, - {31, 24, 32}, - {24, 17, 32}, - {32, 17, 25}, - {25, 26, 33}, - {33, 26, 34}, - {26, 27, 34}, - {34, 27, 35}, - {27, 28, 35}, - {35, 28, 36}, - {28, 29, 36}, - {36, 29, 37}, - {29, 30, 37}, - {37, 30, 38}, - {30, 31, 38}, - {38, 31, 39}, - {31, 32, 39}, - {39, 32, 40}, - {32, 25, 40}, - {40, 25, 33}, - {33, 34, 41}, - {41, 34, 42}, - {34, 35, 42}, - {42, 35, 43}, - {35, 36, 43}, - {43, 36, 44}, - {36, 37, 44}, - {44, 37, 45}, - {37, 38, 45}, - {45, 38, 46}, - {38, 39, 46}, - {46, 39, 47}, - {39, 40, 47}, - {47, 40, 48}, - {40, 33, 48}, - {48, 33, 41}, - {41, 42, 49}, - {49, 42, 50}, - {42, 43, 50}, - {50, 43, 51}, - {43, 44, 51}, - {51, 44, 52}, - {44, 45, 52}, - {52, 45, 53}, - {45, 46, 53}, - {53, 46, 54}, - {46, 47, 54}, - {54, 47, 55}, - {47, 48, 55}, - {55, 48, 56}, - {48, 41, 56}, - {56, 41, 49}, - {2, 1, 57}, - {3, 2, 57}, - {4, 3, 57}, - {5, 4, 57}, - {6, 5, 57}, - {7, 6, 57}, - {8, 7, 57}, - {1, 8, 57}, - {49, 50, 58}, - {50, 51, 58}, - {51, 52, 58}, - {52, 53, 58}, - {53, 54, 58}, - {54, 55, 58}, - {55, 56, 58}, - {56, 49, 58} - }; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 112; i++) - { - p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, - v[f[i][0]][1] * _sizeY, - v[f[i][0]][2] * _sizeZ); - p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, - v[f[i][1]][1] * _sizeY, - v[f[i][1]][2] * _sizeZ); - p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, - v[f[i][2]][1] * _sizeY, - v[f[i][2]][2] * _sizeZ); - - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - - return model; -} - -#endif - -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - -//============================================================================== -template -fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, - double _height, int _slices, int _stacks) -{ - const int CACHE_SIZE = 240; - - int i, j; - float sinCache[CACHE_SIZE]; - float cosCache[CACHE_SIZE]; - float angle; - float zBase; - float zLow, zHigh; - float sintemp, costemp; - float deltaRadius; - float radiusLow, radiusHigh; - - if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; - - if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || - _height < 0.0) - { - return nullptr; - } - - /* Center at CoM */ - zBase = -_height/2; - - /* Compute delta */ - deltaRadius = _baseRadius - _topRadius; - - /* Cache is the vertex locations cache */ - for (i = 0; i < _slices; i++) - { - angle = 2 * M_PI * i / _slices; - sinCache[i] = sin(angle); - cosCache[i] = cos(angle); - } - - sinCache[_slices] = sinCache[0]; - cosCache[_slices] = cosCache[0]; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3, p4; - - model->beginModel(); - - /* Base of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _baseRadius; - zLow = zBase; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - /* Body of cylinder */ - for (i = 0; i < _slices; i++) - { - for (j = 0; j < _stacks; j++) - { - zLow = j * _height / _stacks + zBase; - zHigh = (j + 1) * _height / _stacks + zBase; - radiusLow = _baseRadius - - deltaRadius * (static_cast(j) / _stacks); - radiusHigh = _baseRadius - - deltaRadius * (static_cast(j + 1) / _stacks); - - p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], - zLow); - p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], - zLow); - p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], - zHigh); - p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], - zHigh); - - model->addTriangle(p1, p2, p3); - model->addTriangle(p2, p3, p4); - } - } - - /* Top of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _topRadius; - zLow = zBase + _height; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - return model; -} - -#endif - -//============================================================================== -template -fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, - const aiScene* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - for (size_t i = 0; i < _mesh->mNumMeshes; i++) - { - for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) - { - fcl::Vec3f vertices[3]; - for (size_t k = 0; k < 3; k++) - { - const aiVector3D& vertex - = _mesh->mMeshes[i]->mVertices[ - _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x * _scaleX, - vertex.y * _scaleY, - vertex.z * _scaleZ); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - - for (size_t i = 0; i < _mesh->mNumFaces; i++) - { - fcl::Vec3f vertices[3]; - for (size_t j = 0; j < 3; j++) - { - const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - - model->endModel(); - return model; -} - -//============================================================================== -boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ShapePtr& shape) -{ - using dynamics::Shape; - using dynamics::BoxShape; - using dynamics::EllipsoidShape; - using dynamics::CylinderShape; - using dynamics::PlaneShape; - using dynamics::MeshShape; - using dynamics::SoftMeshShape; - - boost::shared_ptr fclCollGeom; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - const BoxShape* box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset(createCube(size[0], size[1], size[2])); -#else - fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); -#endif - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - EllipsoidShape* ellipsoid = static_cast(shape.get()); - - const Eigen::Vector3d& size = ellipsoid->getSize(); - - if (ellipsoid->isSphere()) - { - fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); - } - else - { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset( - createEllipsoid(ellipsoid->getSize()[0], - ellipsoid->getSize()[1], - ellipsoid->getSize()[2])); -#else - fclCollGeom.reset( - new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); -#endif - } - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - const CylinderShape* cylinder - = static_cast(shape.get()); - const double radius = cylinder->getRadius(); - const double height = cylinder->getHeight(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); -#else - fclCollGeom.reset(new fcl::Cylinder(radius, height)); -#endif - // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 - // returns single contact point for cylinder yet. - break; - } - case Shape::PLANE: - { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); -#else - assert(dynamic_cast(shape.get())); - dynamics::PlaneShape* plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - fclCollGeom.reset( - new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); -#endif - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - MeshShape* shapeMesh = static_cast(shape.get()); - fclCollGeom.reset( - createMesh(shapeMesh->getScale()[0], - shapeMesh->getScale()[1], - shapeMesh->getScale()[2], - shapeMesh->getMesh())); - - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - SoftMeshShape* softMeshShape = static_cast(shape.get()); - fclCollGeom.reset( - createSoftMesh(softMeshShape->getAssimpMesh())); - - break; - } - default: - { - dterr << "[FCLCollisionObjectData::updateShape] " - << "Attempting to create unsupported shape type '" - << shape->getShapeType() << "'.\n"; - return nullptr; - } - } - - return fclCollGeom; -} - -} // anonymous namespace - - //============================================================================== FCLCollisionObjectUserData::FCLCollisionObjectUserData( - CollisionObject* collisionObject, - const dynamics::ShapePtr& shape) - : mCollisionObject(collisionObject), - mShape(shape) + CollisionObject* collisionObject) + : mCollisionObject(collisionObject) { // Do nothing } -//============================================================================== -FCLCollisionObjectData::FCLCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent, - const dynamics::ShapePtr& shape) - : CollisionObjectData(collisionDetector, parent), - mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), - mFCLCollisionObject(new fcl::CollisionObject( - createFCLCollisionGeometry(shape))) -{ - mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); -} - //============================================================================== void FCLCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) { @@ -627,14 +62,6 @@ void FCLCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) mFCLCollisionObject->computeAABB(); } -//============================================================================== -void FCLCollisionObjectData::updateShape(const dynamics::ShapePtr& shape) -{ - auto fclCollGeom = createFCLCollisionGeometry(shape); - - mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); -} - //============================================================================== void FCLCollisionObjectData::update() { @@ -689,5 +116,17 @@ fcl::CollisionObject* FCLCollisionObjectData::getFCLCollisionObject() const return mFCLCollisionObject.get(); } +//============================================================================== +FCLCollisionObjectData::FCLCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent, + const boost::shared_ptr& fclCollGeom) + : CollisionObjectData(collisionDetector, parent), + mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent)), + mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) +{ + mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLCollisionObjectData.h b/dart/collision/fcl/FCLCollisionObjectData.h index 9e9d6306f98a7..b318fa4299324 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.h +++ b/dart/collision/fcl/FCLCollisionObjectData.h @@ -52,35 +52,33 @@ class CollisionObject; struct FCLCollisionObjectUserData { CollisionObject* mCollisionObject; - dynamics::BodyNode* mBodyNode; - dynamics::ShapePtr mShape; - - FCLCollisionObjectUserData(CollisionObject* mFclCollNode, - const dynamics::ShapePtr& mShape); + FCLCollisionObjectUserData(CollisionObject* collisionObject); }; class FCLCollisionObjectData : public CollisionObjectData { public: - /// Constructor - FCLCollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent, - const dynamics::ShapePtr& shape); + friend class FCLCollisionDetector; // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; - // Documentation inherited - void updateShape(const dynamics::ShapePtr& shape) override; - // Documentation inherited void update() override; /// Return FCL collision object fcl::CollisionObject* getFCLCollisionObject() const; +protected: + + /// Constructor + FCLCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent, + const boost::shared_ptr& fclCollGeom); + protected: /// FCL collision geometry user data diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp index d696f9d38a40e..3446589261600 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp @@ -36,25 +36,38 @@ #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include + #include #include #include #include #include +#include +#include +#include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" #include "dart/collision/fcl_mesh/tri_tri_intersection_test.h" #include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" #include "dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/BoxShape.h" +#include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/CylinderShape.h" +#include "dart/dynamics/PlaneShape.h" +#include "dart/dynamics/Shape.h" +#include "dart/dynamics/MeshShape.h" +#include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { namespace { -bool checkPair(fcl::CollisionObject* o1, +bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); @@ -71,7 +84,7 @@ void convertOption(const Option& option, fcl::CollisionRequest& fclRequest); /// Collision data stores the collision request and the result given by /// collision algorithm. -struct FCLCollisionData +struct FCLCollisionCallbackData { /// FCL collision request fcl::CollisionRequest mFclRequest; @@ -89,7 +102,7 @@ struct FCLCollisionData bool done; /// Constructor - FCLCollisionData( + FCLCollisionCallbackData( const Option* option = nullptr, Result* result = nullptr) : mOption(option), @@ -122,6 +135,505 @@ int FFtest( double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); +//============================================================================== +// Create a cube mesh for collision detection +template +fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) +{ + int faces[6][4] = + { + {0, 1, 2, 3}, + {3, 2, 6, 7}, + {7, 6, 5, 4}, + {4, 5, 1, 0}, + {5, 6, 2, 1}, + {7, 4, 0, 3} + }; + float v[8][3]; + + v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; + v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; + v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; + v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; + v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; + v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 6; i++) + { + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + model->addTriangle(p1, p2, p3); + + p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + model->addTriangle(p1, p2, p3); + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) +{ + float v[59][3] = + { + {0, 0, 0}, + {0.135299, -0.461940, -0.135299}, + {0.000000, -0.461940, -0.191342}, + {-0.135299, -0.461940, -0.135299}, + {-0.191342, -0.461940, 0.000000}, + {-0.135299, -0.461940, 0.135299}, + {0.000000, -0.461940, 0.191342}, + {0.135299, -0.461940, 0.135299}, + {0.191342, -0.461940, 0.000000}, + {0.250000, -0.353553, -0.250000}, + {0.000000, -0.353553, -0.353553}, + {-0.250000, -0.353553, -0.250000}, + {-0.353553, -0.353553, 0.000000}, + {-0.250000, -0.353553, 0.250000}, + {0.000000, -0.353553, 0.353553}, + {0.250000, -0.353553, 0.250000}, + {0.353553, -0.353553, 0.000000}, + {0.326641, -0.191342, -0.326641}, + {0.000000, -0.191342, -0.461940}, + {-0.326641, -0.191342, -0.326641}, + {-0.461940, -0.191342, 0.000000}, + {-0.326641, -0.191342, 0.326641}, + {0.000000, -0.191342, 0.461940}, + {0.326641, -0.191342, 0.326641}, + {0.461940, -0.191342, 0.000000}, + {0.353553, 0.000000, -0.353553}, + {0.000000, 0.000000, -0.500000}, + {-0.353553, 0.000000, -0.353553}, + {-0.500000, 0.000000, 0.000000}, + {-0.353553, 0.000000, 0.353553}, + {0.000000, 0.000000, 0.500000}, + {0.353553, 0.000000, 0.353553}, + {0.500000, 0.000000, 0.000000}, + {0.326641, 0.191342, -0.326641}, + {0.000000, 0.191342, -0.461940}, + {-0.326641, 0.191342, -0.326641}, + {-0.461940, 0.191342, 0.000000}, + {-0.326641, 0.191342, 0.326641}, + {0.000000, 0.191342, 0.461940}, + {0.326641, 0.191342, 0.326641}, + {0.461940, 0.191342, 0.000000}, + {0.250000, 0.353553, -0.250000}, + {0.000000, 0.353553, -0.353553}, + {-0.250000, 0.353553, -0.250000}, + {-0.353553, 0.353553, 0.000000}, + {-0.250000, 0.353553, 0.250000}, + {0.000000, 0.353553, 0.353553}, + {0.250000, 0.353553, 0.250000}, + {0.353553, 0.353553, 0.000000}, + {0.135299, 0.461940, -0.135299}, + {0.000000, 0.461940, -0.191342}, + {-0.135299, 0.461940, -0.135299}, + {-0.191342, 0.461940, 0.000000}, + {-0.135299, 0.461940, 0.135299}, + {0.000000, 0.461940, 0.191342}, + {0.135299, 0.461940, 0.135299}, + {0.191342, 0.461940, 0.000000}, + {0.000000, -0.500000, 0.000000}, + {0.000000, 0.500000, 0.000000} + }; + + int f[112][3] = + { + {1, 2, 9}, + {9, 2, 10}, + {2, 3, 10}, + {10, 3, 11}, + {3, 4, 11}, + {11, 4, 12}, + {4, 5, 12}, + {12, 5, 13}, + {5, 6, 13}, + {13, 6, 14}, + {6, 7, 14}, + {14, 7, 15}, + {7, 8, 15}, + {15, 8, 16}, + {8, 1, 16}, + {16, 1, 9}, + {9, 10, 17}, + {17, 10, 18}, + {10, 11, 18}, + {18, 11, 19}, + {11, 12, 19}, + {19, 12, 20}, + {12, 13, 20}, + {20, 13, 21}, + {13, 14, 21}, + {21, 14, 22}, + {14, 15, 22}, + {22, 15, 23}, + {15, 16, 23}, + {23, 16, 24}, + {16, 9, 24}, + {24, 9, 17}, + {17, 18, 25}, + {25, 18, 26}, + {18, 19, 26}, + {26, 19, 27}, + {19, 20, 27}, + {27, 20, 28}, + {20, 21, 28}, + {28, 21, 29}, + {21, 22, 29}, + {29, 22, 30}, + {22, 23, 30}, + {30, 23, 31}, + {23, 24, 31}, + {31, 24, 32}, + {24, 17, 32}, + {32, 17, 25}, + {25, 26, 33}, + {33, 26, 34}, + {26, 27, 34}, + {34, 27, 35}, + {27, 28, 35}, + {35, 28, 36}, + {28, 29, 36}, + {36, 29, 37}, + {29, 30, 37}, + {37, 30, 38}, + {30, 31, 38}, + {38, 31, 39}, + {31, 32, 39}, + {39, 32, 40}, + {32, 25, 40}, + {40, 25, 33}, + {33, 34, 41}, + {41, 34, 42}, + {34, 35, 42}, + {42, 35, 43}, + {35, 36, 43}, + {43, 36, 44}, + {36, 37, 44}, + {44, 37, 45}, + {37, 38, 45}, + {45, 38, 46}, + {38, 39, 46}, + {46, 39, 47}, + {39, 40, 47}, + {47, 40, 48}, + {40, 33, 48}, + {48, 33, 41}, + {41, 42, 49}, + {49, 42, 50}, + {42, 43, 50}, + {50, 43, 51}, + {43, 44, 51}, + {51, 44, 52}, + {44, 45, 52}, + {52, 45, 53}, + {45, 46, 53}, + {53, 46, 54}, + {46, 47, 54}, + {54, 47, 55}, + {47, 48, 55}, + {55, 48, 56}, + {48, 41, 56}, + {56, 41, 49}, + {2, 1, 57}, + {3, 2, 57}, + {4, 3, 57}, + {5, 4, 57}, + {6, 5, 57}, + {7, 6, 57}, + {8, 7, 57}, + {1, 8, 57}, + {49, 50, 58}, + {50, 51, 58}, + {51, 52, 58}, + {52, 53, 58}, + {53, 54, 58}, + {54, 55, 58}, + {55, 56, 58}, + {56, 49, 58} + }; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3; + model->beginModel(); + + for (int i = 0; i < 112; i++) + { + p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, + v[f[i][0]][1] * _sizeY, + v[f[i][0]][2] * _sizeZ); + p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, + v[f[i][1]][1] * _sizeY, + v[f[i][1]][2] * _sizeZ); + p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, + v[f[i][2]][1] * _sizeY, + v[f[i][2]][2] * _sizeZ); + + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + + return model; +} + +//============================================================================== +template +fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, + double _height, int _slices, int _stacks) +{ + const int CACHE_SIZE = 240; + + int i, j; + float sinCache[CACHE_SIZE]; + float cosCache[CACHE_SIZE]; + float angle; + float zBase; + float zLow, zHigh; + float sintemp, costemp; + float deltaRadius; + float radiusLow, radiusHigh; + + if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; + + if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || + _height < 0.0) + { + return nullptr; + } + + /* Center at CoM */ + zBase = -_height/2; + + /* Compute delta */ + deltaRadius = _baseRadius - _topRadius; + + /* Cache is the vertex locations cache */ + for (i = 0; i < _slices; i++) + { + angle = 2 * M_PI * i / _slices; + sinCache[i] = sin(angle); + cosCache[i] = cos(angle); + } + + sinCache[_slices] = sinCache[0]; + cosCache[_slices] = cosCache[0]; + + fcl::BVHModel* model = new fcl::BVHModel; + fcl::Vec3f p1, p2, p3, p4; + + model->beginModel(); + + /* Base of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _baseRadius; + zLow = zBase; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + /* Body of cylinder */ + for (i = 0; i < _slices; i++) + { + for (j = 0; j < _stacks; j++) + { + zLow = j * _height / _stacks + zBase; + zHigh = (j + 1) * _height / _stacks + zBase; + radiusLow = _baseRadius + - deltaRadius * (static_cast(j) / _stacks); + radiusHigh = _baseRadius + - deltaRadius * (static_cast(j + 1) / _stacks); + + p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); + + model->addTriangle(p1, p2, p3); + model->addTriangle(p2, p3, p4); + } + } + + /* Top of cylinder */ + sintemp = sinCache[0]; + costemp = cosCache[0]; + radiusLow = _topRadius; + zLow = zBase + _height; + p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + for (i = 1; i < _slices; i++) + { + p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + model->addTriangle(p1, p2, p3); + } + + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, + const aiScene* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + for (size_t i = 0; i < _mesh->mNumMeshes; i++) + { + for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) + { + fcl::Vec3f vertices[3]; + for (size_t k = 0; k < 3; k++) + { + const aiVector3D& vertex + = _mesh->mMeshes[i]->mVertices[ + _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + vertices[k] = fcl::Vec3f(vertex.x * _scaleX, + vertex.y * _scaleY, + vertex.z * _scaleZ); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + } + model->endModel(); + return model; +} + +//============================================================================== +template +fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) +{ + // Create FCL mesh from Assimp mesh + + assert(_mesh); + fcl::BVHModel* model = new fcl::BVHModel; + model->beginModel(); + + for (size_t i = 0; i < _mesh->mNumFaces; i++) + { + fcl::Vec3f vertices[3]; + for (size_t j = 0; j < 3; j++) + { + const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; + vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + } + model->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + model->endModel(); + return model; +} + +//============================================================================== +boost::shared_ptr createFCLCollisionGeometry( + const dynamics::ShapePtr& shape) +{ + using ::dart::dynamics::Shape; + using ::dart::dynamics::BoxShape; + using ::dart::dynamics::EllipsoidShape; + using ::dart::dynamics::CylinderShape; + using ::dart::dynamics::PlaneShape; + using ::dart::dynamics::MeshShape; + using ::dart::dynamics::SoftMeshShape; + + boost::shared_ptr fclCollGeom; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + const BoxShape* box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); + + auto model = new fcl::BVHModel; + fcl::generateBVHModel(*model, fcl::Box(size[0], size[1], size[2]), fcl::Transform3f()); + + fclCollGeom.reset(createCube(size[0], size[1], size[2])); +// fclCollGeom.reset(model); + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + EllipsoidShape* ellipsoid = static_cast(shape.get()); + + const Eigen::Vector3d& size = ellipsoid->getSize(); + + fclCollGeom.reset( + createEllipsoid(size[0], size[1], size[2])); + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + const CylinderShape* cylinder + = static_cast(shape.get()); + const double radius = cylinder->getRadius(); + const double height = cylinder->getHeight(); + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); + // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 + // returns single contact point for cylinder yet. + break; + } + case Shape::PLANE: + { + fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + MeshShape* shapeMesh = static_cast(shape.get()); + fclCollGeom.reset(createMesh(shapeMesh->getScale()[0], + shapeMesh->getScale()[1], + shapeMesh->getScale()[2], + shapeMesh->getMesh())); + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + SoftMeshShape* softMeshShape = static_cast(shape.get()); + fclCollGeom.reset( + createSoftMesh(softMeshShape->getAssimpMesh())); + + break; + } + default: + { + dterr << "[FCLMeshCollisionObjectData::updateShape] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + return nullptr; + } + } + + return fclCollGeom; +} + } // anonymous namespace @@ -133,6 +645,12 @@ std::shared_ptr FCLMeshCollisionDetector::create() new FCLMeshCollisionDetector()); } +//============================================================================== +FCLMeshCollisionDetector::~FCLMeshCollisionDetector() +{ + assert(mShapeMap.empty()); +} + //============================================================================== const std::string& FCLMeshCollisionDetector::getType() const { @@ -152,8 +670,41 @@ FCLMeshCollisionDetector::createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) { + boost::shared_ptr fclCollGeom; + + auto findResult = mShapeMap.find(shape); + if (mShapeMap.end() != findResult) + { + fclCollGeom = findResult->second.first; + } + else + { + fclCollGeom = createFCLCollisionGeometry(shape); + mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); + } + return std::unique_ptr( - new FCLMeshCollisionObjectData(this, parent, shape)); + new FCLMeshCollisionObjectData(this, parent, fclCollGeom)); +} + +//============================================================================== +void FCLMeshCollisionDetector::reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) +{ + // Retrieve associated shape + auto shape = collisionObjectData->getCollisionObject()->getShape(); + assert(shape); + + auto findResult = mShapeMap.find(shape); + assert(mShapeMap.end() != findResult); + + auto fclCollGeomAndCount = findResult->second; + assert(0u != fclCollGeomAndCount.second); + + fclCollGeomAndCount.second--; + + if (0u == fclCollGeomAndCount.second) + mShapeMap.erase(findResult); } //============================================================================== @@ -183,8 +734,8 @@ bool FCLMeshCollisionDetector::detect( auto fclCollObj1 = castedData1->getFCLCollisionObject(); auto fclCollObj2 = castedData2->getFCLCollisionObject(); - FCLCollisionData collData(&option, &result); - checkPair(fclCollObj1, fclCollObj2, &collData); + FCLCollisionCallbackData collData(&option, &result); + collisionCallback(fclCollObj1, fclCollObj2, &collData); return !result.contacts.empty(); } @@ -208,8 +759,8 @@ bool FCLMeshCollisionDetector::detect( auto fclObject = castedObjData->getFCLCollisionObject(); auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); - FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(fclObject, &collData, checkPair); + FCLCollisionCallbackData collData(&option, &result); + broadPhaseAlg->collide(fclObject, &collData, collisionCallback); return !result.contacts.empty(); } @@ -228,8 +779,8 @@ bool FCLMeshCollisionDetector::detect( auto broadPhaseAlg = castedData->getFCLCollisionManager(); - FCLCollisionData collData(&option, &result); - broadPhaseAlg->collide(&collData, checkPair); + FCLCollisionCallbackData collData(&option, &result); + broadPhaseAlg->collide(&collData, collisionCallback); return !result.contacts.empty(); } @@ -253,8 +804,8 @@ bool FCLMeshCollisionDetector::detect( auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); - FCLCollisionData collData(&option, &result); - broadPhaseAlg1->collide(broadPhaseAlg2, &collData, checkPair); + FCLCollisionCallbackData collData(&option, &result); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); return !result.contacts.empty(); } @@ -264,11 +815,11 @@ bool FCLMeshCollisionDetector::detect( namespace { //============================================================================== -bool checkPair(fcl::CollisionObject* o1, +bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { - FCLCollisionData* collData = static_cast(cdata); + FCLCollisionCallbackData* collData = static_cast(cdata); const fcl::CollisionRequest& fclRequest = collData->mFclRequest; fcl::CollisionResult& fclResult = collData->mFclResult; diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h index c934c81464431..415cdc6c7cc5e 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h @@ -38,7 +38,7 @@ #define DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ #include - +#include #include "dart/collision/CollisionDetector.h" namespace dart { @@ -50,10 +50,13 @@ class FCLMeshCollisionDetector : public CollisionDetector static std::shared_ptr create(); + /// Constructor + virtual ~FCLMeshCollisionDetector(); + /// Return engine type "FCLMesh" static const std::string& getTypeStatic(); - // Documentation inherit + // Documentation inherited const std::string& getType() const override; using CollisionDetector::detect; @@ -63,32 +66,43 @@ class FCLMeshCollisionDetector : public CollisionDetector /// Constructor FCLMeshCollisionDetector() = default; - // Documentation inherit + // Documentation inherited std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) override; - // Documentation inherit + // Documentation inherited + void reclaimCollisionObjectData( + CollisionObjectData* collisionObjectData) override; + + // Documentation inherited std::unique_ptr createCollisionGroupData( CollisionGroup* parent, const CollisionObjectPtrs& collObjects) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object1, CollisionObjectData* object2, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionObjectData* object, CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group, const Option& option, Result& result) override; - // Documentation inherit + // Documentation inherited bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; +protected: + + using ShapeMapValue + = std::pair, size_t>; + + std::map mShapeMap; + }; } // namespace collision diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp index dd6b1cabf9f1b..784a83817b4d9 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp @@ -36,544 +36,17 @@ #include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" -#include #include -#include -#include -#include "dart/common/Console.h" #include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { -namespace { - -//============================================================================== -// Create a cube mesh for collision detection -template -fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) -{ - int faces[6][4] = - { - {0, 1, 2, 3}, - {3, 2, 6, 7}, - {7, 6, 5, 4}, - {4, 5, 1, 0}, - {5, 6, 2, 1}, - {7, 4, 0, 3} - }; - float v[8][3]; - - v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; - v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; - v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; - v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; - v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; - v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 6; i++) - { - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); - p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - model->addTriangle(p1, p2, p3); - - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); - model->addTriangle(p1, p2, p3); - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) -{ - float v[59][3] = - { - {0, 0, 0}, - {0.135299, -0.461940, -0.135299}, - {0.000000, -0.461940, -0.191342}, - {-0.135299, -0.461940, -0.135299}, - {-0.191342, -0.461940, 0.000000}, - {-0.135299, -0.461940, 0.135299}, - {0.000000, -0.461940, 0.191342}, - {0.135299, -0.461940, 0.135299}, - {0.191342, -0.461940, 0.000000}, - {0.250000, -0.353553, -0.250000}, - {0.000000, -0.353553, -0.353553}, - {-0.250000, -0.353553, -0.250000}, - {-0.353553, -0.353553, 0.000000}, - {-0.250000, -0.353553, 0.250000}, - {0.000000, -0.353553, 0.353553}, - {0.250000, -0.353553, 0.250000}, - {0.353553, -0.353553, 0.000000}, - {0.326641, -0.191342, -0.326641}, - {0.000000, -0.191342, -0.461940}, - {-0.326641, -0.191342, -0.326641}, - {-0.461940, -0.191342, 0.000000}, - {-0.326641, -0.191342, 0.326641}, - {0.000000, -0.191342, 0.461940}, - {0.326641, -0.191342, 0.326641}, - {0.461940, -0.191342, 0.000000}, - {0.353553, 0.000000, -0.353553}, - {0.000000, 0.000000, -0.500000}, - {-0.353553, 0.000000, -0.353553}, - {-0.500000, 0.000000, 0.000000}, - {-0.353553, 0.000000, 0.353553}, - {0.000000, 0.000000, 0.500000}, - {0.353553, 0.000000, 0.353553}, - {0.500000, 0.000000, 0.000000}, - {0.326641, 0.191342, -0.326641}, - {0.000000, 0.191342, -0.461940}, - {-0.326641, 0.191342, -0.326641}, - {-0.461940, 0.191342, 0.000000}, - {-0.326641, 0.191342, 0.326641}, - {0.000000, 0.191342, 0.461940}, - {0.326641, 0.191342, 0.326641}, - {0.461940, 0.191342, 0.000000}, - {0.250000, 0.353553, -0.250000}, - {0.000000, 0.353553, -0.353553}, - {-0.250000, 0.353553, -0.250000}, - {-0.353553, 0.353553, 0.000000}, - {-0.250000, 0.353553, 0.250000}, - {0.000000, 0.353553, 0.353553}, - {0.250000, 0.353553, 0.250000}, - {0.353553, 0.353553, 0.000000}, - {0.135299, 0.461940, -0.135299}, - {0.000000, 0.461940, -0.191342}, - {-0.135299, 0.461940, -0.135299}, - {-0.191342, 0.461940, 0.000000}, - {-0.135299, 0.461940, 0.135299}, - {0.000000, 0.461940, 0.191342}, - {0.135299, 0.461940, 0.135299}, - {0.191342, 0.461940, 0.000000}, - {0.000000, -0.500000, 0.000000}, - {0.000000, 0.500000, 0.000000} - }; - - int f[112][3] = - { - {1, 2, 9}, - {9, 2, 10}, - {2, 3, 10}, - {10, 3, 11}, - {3, 4, 11}, - {11, 4, 12}, - {4, 5, 12}, - {12, 5, 13}, - {5, 6, 13}, - {13, 6, 14}, - {6, 7, 14}, - {14, 7, 15}, - {7, 8, 15}, - {15, 8, 16}, - {8, 1, 16}, - {16, 1, 9}, - {9, 10, 17}, - {17, 10, 18}, - {10, 11, 18}, - {18, 11, 19}, - {11, 12, 19}, - {19, 12, 20}, - {12, 13, 20}, - {20, 13, 21}, - {13, 14, 21}, - {21, 14, 22}, - {14, 15, 22}, - {22, 15, 23}, - {15, 16, 23}, - {23, 16, 24}, - {16, 9, 24}, - {24, 9, 17}, - {17, 18, 25}, - {25, 18, 26}, - {18, 19, 26}, - {26, 19, 27}, - {19, 20, 27}, - {27, 20, 28}, - {20, 21, 28}, - {28, 21, 29}, - {21, 22, 29}, - {29, 22, 30}, - {22, 23, 30}, - {30, 23, 31}, - {23, 24, 31}, - {31, 24, 32}, - {24, 17, 32}, - {32, 17, 25}, - {25, 26, 33}, - {33, 26, 34}, - {26, 27, 34}, - {34, 27, 35}, - {27, 28, 35}, - {35, 28, 36}, - {28, 29, 36}, - {36, 29, 37}, - {29, 30, 37}, - {37, 30, 38}, - {30, 31, 38}, - {38, 31, 39}, - {31, 32, 39}, - {39, 32, 40}, - {32, 25, 40}, - {40, 25, 33}, - {33, 34, 41}, - {41, 34, 42}, - {34, 35, 42}, - {42, 35, 43}, - {35, 36, 43}, - {43, 36, 44}, - {36, 37, 44}, - {44, 37, 45}, - {37, 38, 45}, - {45, 38, 46}, - {38, 39, 46}, - {46, 39, 47}, - {39, 40, 47}, - {47, 40, 48}, - {40, 33, 48}, - {48, 33, 41}, - {41, 42, 49}, - {49, 42, 50}, - {42, 43, 50}, - {50, 43, 51}, - {43, 44, 51}, - {51, 44, 52}, - {44, 45, 52}, - {52, 45, 53}, - {45, 46, 53}, - {53, 46, 54}, - {46, 47, 54}, - {54, 47, 55}, - {47, 48, 55}, - {55, 48, 56}, - {48, 41, 56}, - {56, 41, 49}, - {2, 1, 57}, - {3, 2, 57}, - {4, 3, 57}, - {5, 4, 57}, - {6, 5, 57}, - {7, 6, 57}, - {8, 7, 57}, - {1, 8, 57}, - {49, 50, 58}, - {50, 51, 58}, - {51, 52, 58}, - {52, 53, 58}, - {53, 54, 58}, - {54, 55, 58}, - {55, 56, 58}, - {56, 49, 58} - }; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 112; i++) - { - p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, - v[f[i][0]][1] * _sizeY, - v[f[i][0]][2] * _sizeZ); - p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, - v[f[i][1]][1] * _sizeY, - v[f[i][1]][2] * _sizeZ); - p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, - v[f[i][2]][1] * _sizeY, - v[f[i][2]][2] * _sizeZ); - - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - - return model; -} - -//============================================================================== -template -fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, - double _height, int _slices, int _stacks) -{ - const int CACHE_SIZE = 240; - - int i, j; - float sinCache[CACHE_SIZE]; - float cosCache[CACHE_SIZE]; - float angle; - float zBase; - float zLow, zHigh; - float sintemp, costemp; - float deltaRadius; - float radiusLow, radiusHigh; - - if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; - - if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || - _height < 0.0) - { - return nullptr; - } - - /* Center at CoM */ - zBase = -_height/2; - - /* Compute delta */ - deltaRadius = _baseRadius - _topRadius; - - /* Cache is the vertex locations cache */ - for (i = 0; i < _slices; i++) - { - angle = 2 * M_PI * i / _slices; - sinCache[i] = sin(angle); - cosCache[i] = cos(angle); - } - - sinCache[_slices] = sinCache[0]; - cosCache[_slices] = cosCache[0]; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3, p4; - - model->beginModel(); - - /* Base of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _baseRadius; - zLow = zBase; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - /* Body of cylinder */ - for (i = 0; i < _slices; i++) - { - for (j = 0; j < _stacks; j++) - { - zLow = j * _height / _stacks + zBase; - zHigh = (j + 1) * _height / _stacks + zBase; - radiusLow = _baseRadius - - deltaRadius * (static_cast(j) / _stacks); - radiusHigh = _baseRadius - - deltaRadius * (static_cast(j + 1) / _stacks); - - p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], - zLow); - p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], - zLow); - p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], - zHigh); - p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], - zHigh); - - model->addTriangle(p1, p2, p3); - model->addTriangle(p2, p3, p4); - } - } - - /* Top of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _topRadius; - zLow = zBase + _height; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, - const aiScene* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - for (size_t i = 0; i < _mesh->mNumMeshes; i++) - { - for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) - { - fcl::Vec3f vertices[3]; - for (size_t k = 0; k < 3; k++) - { - const aiVector3D& vertex - = _mesh->mMeshes[i]->mVertices[ - _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x * _scaleX, - vertex.y * _scaleY, - vertex.z * _scaleZ); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - - for (size_t i = 0; i < _mesh->mNumFaces; i++) - { - fcl::Vec3f vertices[3]; - for (size_t j = 0; j < 3; j++) - { - const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - - model->endModel(); - return model; -} - -//============================================================================== -boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ShapePtr& shape) -{ - using ::dart::dynamics::Shape; - using ::dart::dynamics::BoxShape; - using ::dart::dynamics::EllipsoidShape; - using ::dart::dynamics::CylinderShape; - using ::dart::dynamics::PlaneShape; - using ::dart::dynamics::MeshShape; - using ::dart::dynamics::SoftMeshShape; - - boost::shared_ptr fclCollGeom; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - const BoxShape* box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); - - auto model = new fcl::BVHModel; - fcl::generateBVHModel(*model, fcl::Box(size[0], size[1], size[2]), fcl::Transform3f()); - - fclCollGeom.reset(createCube(size[0], size[1], size[2])); -// fclCollGeom.reset(model); - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - EllipsoidShape* ellipsoid = static_cast(shape.get()); - - const Eigen::Vector3d& size = ellipsoid->getSize(); - - fclCollGeom.reset( - createEllipsoid(size[0], size[1], size[2])); - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - const CylinderShape* cylinder - = static_cast(shape.get()); - const double radius = cylinder->getRadius(); - const double height = cylinder->getHeight(); - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); - // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 - // returns single contact point for cylinder yet. - break; - } - case Shape::PLANE: - { - fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - MeshShape* shapeMesh = static_cast(shape.get()); - fclCollGeom.reset(createMesh(shapeMesh->getScale()[0], - shapeMesh->getScale()[1], - shapeMesh->getScale()[2], - shapeMesh->getMesh())); - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - SoftMeshShape* softMeshShape = static_cast(shape.get()); - fclCollGeom.reset( - createSoftMesh(softMeshShape->getAssimpMesh())); - - break; - } - default: - { - dterr << "[FCLMeshCollisionObjectData::updateShape] " - << "Attempting to create unsupported shape type '" - << shape->getShapeType() << "'.\n"; - return nullptr; - } - } - - return fclCollGeom; -} - -} // anonymous namespace - -//============================================================================== -FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent, - const dynamics::ShapePtr& shape) - : CollisionObjectData(collisionDetector, parent), - mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent, shape)), - mFCLCollisionObject(new fcl::CollisionObject( - createFCLCollisionGeometry(shape))) -{ - mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); -} - //============================================================================== void FCLMeshCollisionObjectData::updateTransform( const Eigen::Isometry3d& tf) @@ -582,16 +55,6 @@ void FCLMeshCollisionObjectData::updateTransform( mFCLCollisionObject->computeAABB(); } -//============================================================================== -void FCLMeshCollisionObjectData::updateShape( - const dynamics::ShapePtr& shape) -{ - auto fclCollGeom = createFCLCollisionGeometry(shape); - - mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); - mFCLCollisionObject.reset(new fcl::CollisionObject(fclCollGeom)); -} - //============================================================================== void FCLMeshCollisionObjectData::update() { @@ -649,5 +112,17 @@ fcl::CollisionObject* FCLMeshCollisionObjectData::getFCLCollisionObject() const return mFCLCollisionObject.get(); } +//============================================================================== +FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent, + const boost::shared_ptr& fclCollGeom) + : CollisionObjectData(collisionDetector, parent), + mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent)), + mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) +{ + mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h index 0ccf7301948c6..1628aeaeb5576 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h +++ b/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h @@ -54,23 +54,25 @@ class FCLMeshCollisionObjectData : public CollisionObjectData { public: - /// Constructor - FCLMeshCollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent, - const dynamics::ShapePtr& shape); + friend class FCLMeshCollisionDetector; // Documentation inherited void updateTransform(const Eigen::Isometry3d& tf) override; - // Documentation inherited - void updateShape(const dynamics::ShapePtr& shape) override; - // Documentation inherited void update() override; /// Return FCL collision object fcl::CollisionObject* getFCLCollisionObject() const; +protected: + + /// Constructor + FCLMeshCollisionObjectData( + CollisionDetector* collisionDetector, + CollisionObject* parent, + const boost::shared_ptr& fclCollGeom); + protected: /// FCL collision geometry user data diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 8f186b64083e5..006b9d5ede77f 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -543,8 +543,11 @@ void testFreeCollisionObjects() EXPECT_FALSE(obj2->detect(obj3.get(), option, result)); EXPECT_FALSE(obj1->detect(group2.get(), option, result)); EXPECT_FALSE(group1->detect(obj3.get(), option, result)); -// EXPECT_FALSE(group1->detect(group2.get(), option, result)); - EXPECT_FALSE(group2->detect(option, result)); + if (cd->getType() != "Bullet") + { + EXPECT_FALSE(group1->detect(group2.get(), option, result)); + EXPECT_FALSE(group2->detect(option, result)); + } obj1->setTranslation(Eigen::Vector3d::Zero()); obj2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); @@ -552,28 +555,29 @@ void testFreeCollisionObjects() EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); EXPECT_TRUE(obj2->detect(obj3.get(), option, result)); EXPECT_TRUE(obj1->detect(group2.get(), option, result)); -// EXPECT_TRUE(group1->detect(group2.get(), option, result)); + if (cd->getType() != "Bullet") + EXPECT_TRUE(group1->detect(group2.get(), option, result)); EXPECT_TRUE(group2->detect(option, result)); EXPECT_TRUE(group2->detect(option, result)); -// for (auto contact : result.contacts) -// { -// auto freeCollObj1 = dynamic_cast( -// contact.collisionObject1); -// auto freeCollObj2 = dynamic_cast( -// contact.collisionObject2); + for (auto contact : result.contacts) + { + auto freeCollObj1 = dynamic_cast( + contact.collisionObject1); + auto freeCollObj2 = dynamic_cast( + contact.collisionObject2); -// EXPECT_NE(freeCollObj1, nullptr); -// EXPECT_NE(freeCollObj2, nullptr); -// } + EXPECT_NE(freeCollObj1, nullptr); + EXPECT_NE(freeCollObj2, nullptr); + } } //============================================================================== TEST_F(COLLISION, FreeCollisionObjects) { -// testFreeCollisionObjects(); -// testFreeCollisionObjects(); -// testFreeCollisionObjects(); + testFreeCollisionObjects(); + testFreeCollisionObjects(); + testFreeCollisionObjects(); testFreeCollisionObjects(); } @@ -614,7 +618,7 @@ TEST_F(COLLISION, BodyNodeNodes) testBodyNodes(); testBodyNodes(); testBodyNodes(); -// testBodyNodes(); + testBodyNodes(); } //============================================================================== @@ -644,7 +648,7 @@ TEST_F(COLLISION, Skeletons) testSkeletons(); testSkeletons(); testSkeletons(); -// testSkeletons(); + testSkeletons(); } //============================================================================== From 22fb8f85e020d230160b8bb214a53b477ed73fff Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 1 Mar 2016 19:52:44 -0500 Subject: [PATCH 13/67] Merge 'fcl_mesh' directory into 'fcl' --- dart/collision/CMakeLists.txt | 3 -- .../{fcl_mesh => fcl}/CollisionShapes.h | 0 .../FCLMeshCollisionDetector.cpp | 8 ++--- .../FCLMeshCollisionDetector.h | 0 .../FCLMeshCollisionGroupData.cpp | 4 +-- .../FCLMeshCollisionGroupData.h | 0 .../FCLMeshCollisionObjectData.cpp | 2 +- .../FCLMeshCollisionObjectData.h | 0 .../tri_tri_intersection_test.h | 0 dart/collision/fcl_mesh/CMakeLists.txt | 33 ------------------- dart/collision/fcl_mesh/fcl_mesh.h.in | 3 -- dart/constraint/ConstraintSolver.cpp | 2 +- dart/utils/SkelParser.cpp | 2 +- unittests/testCollision.cpp | 2 +- 14 files changed, 10 insertions(+), 49 deletions(-) rename dart/collision/{fcl_mesh => fcl}/CollisionShapes.h (100%) rename dart/collision/{fcl_mesh => fcl}/FCLMeshCollisionDetector.cpp (99%) rename dart/collision/{fcl_mesh => fcl}/FCLMeshCollisionDetector.h (100%) rename dart/collision/{fcl_mesh => fcl}/FCLMeshCollisionGroupData.cpp (97%) rename dart/collision/{fcl_mesh => fcl}/FCLMeshCollisionGroupData.h (100%) rename dart/collision/{fcl_mesh => fcl}/FCLMeshCollisionObjectData.cpp (98%) rename dart/collision/{fcl_mesh => fcl}/FCLMeshCollisionObjectData.h (100%) rename dart/collision/{fcl_mesh => fcl}/tri_tri_intersection_test.h (100%) delete mode 100644 dart/collision/fcl_mesh/CMakeLists.txt delete mode 100644 dart/collision/fcl_mesh/fcl_mesh.h.in diff --git a/dart/collision/CMakeLists.txt b/dart/collision/CMakeLists.txt index 315cf8a2d9da7..e80d01296c9d7 100644 --- a/dart/collision/CMakeLists.txt +++ b/dart/collision/CMakeLists.txt @@ -7,7 +7,6 @@ file(GLOB detail_hdrs "detail/*.h") # Add subdirectories add_subdirectory(dart) add_subdirectory(fcl) -add_subdirectory(fcl_mesh) if(HAVE_BULLET_COLLISION) add_subdirectory(bullet) endif() @@ -23,7 +22,6 @@ set(dart_collision_srcs ${srcs} "${dart_collision_srcs};${detail_srcs}" PARENT_S # dart_math # dart_collision_dart # dart_collision_fcl -# dart_collision_fcl_mesh # ${DART_CORE_DEPENDENCIES} #) #if(HAVE_BULLET_COLLISION) @@ -37,7 +35,6 @@ set( ${header_names} dart/dart.h fcl/fcl.h - fcl_mesh/fcl_mesh.h ) if(HAVE_BULLET_COLLISION) set(header_names ${header_names} bullet/bullet.h) diff --git a/dart/collision/fcl_mesh/CollisionShapes.h b/dart/collision/fcl/CollisionShapes.h similarity index 100% rename from dart/collision/fcl_mesh/CollisionShapes.h rename to dart/collision/fcl/CollisionShapes.h diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp similarity index 99% rename from dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp rename to dart/collision/fcl/FCLMeshCollisionDetector.cpp index 3446589261600..e9935de31b234 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl/FCLMeshCollisionDetector.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/fcl/FCLMeshCollisionDetector.h" #include @@ -50,9 +50,9 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObjectData.h" -#include "dart/collision/fcl_mesh/tri_tri_intersection_test.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h" +#include "dart/collision/fcl/tri_tri_intersection_test.h" +#include "dart/collision/fcl/FCLMeshCollisionObjectData.h" +#include "dart/collision/fcl/FCLMeshCollisionGroupData.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.h b/dart/collision/fcl/FCLMeshCollisionDetector.h similarity index 100% rename from dart/collision/fcl_mesh/FCLMeshCollisionDetector.h rename to dart/collision/fcl/FCLMeshCollisionDetector.h diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp b/dart/collision/fcl/FCLMeshCollisionGroupData.cpp similarity index 97% rename from dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp rename to dart/collision/fcl/FCLMeshCollisionGroupData.cpp index 28ac1b1b90104..3c09df2948e49 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.cpp +++ b/dart/collision/fcl/FCLMeshCollisionGroupData.cpp @@ -34,10 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h" +#include "dart/collision/fcl/FCLMeshCollisionGroupData.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" +#include "dart/collision/fcl/FCLMeshCollisionObjectData.h" namespace dart { namespace collision { diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h b/dart/collision/fcl/FCLMeshCollisionGroupData.h similarity index 100% rename from dart/collision/fcl_mesh/FCLMeshCollisionGroupData.h rename to dart/collision/fcl/FCLMeshCollisionGroupData.h diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp b/dart/collision/fcl/FCLMeshCollisionObjectData.cpp similarity index 98% rename from dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp rename to dart/collision/fcl/FCLMeshCollisionObjectData.cpp index 784a83817b4d9..760118549248f 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLMeshCollisionObjectData.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h" +#include "dart/collision/fcl/FCLMeshCollisionObjectData.h" #include diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h b/dart/collision/fcl/FCLMeshCollisionObjectData.h similarity index 100% rename from dart/collision/fcl_mesh/FCLMeshCollisionObjectData.h rename to dart/collision/fcl/FCLMeshCollisionObjectData.h diff --git a/dart/collision/fcl_mesh/tri_tri_intersection_test.h b/dart/collision/fcl/tri_tri_intersection_test.h similarity index 100% rename from dart/collision/fcl_mesh/tri_tri_intersection_test.h rename to dart/collision/fcl/tri_tri_intersection_test.h diff --git a/dart/collision/fcl_mesh/CMakeLists.txt b/dart/collision/fcl_mesh/CMakeLists.txt deleted file mode 100644 index 2a70755e99546..0000000000000 --- a/dart/collision/fcl_mesh/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -# Search all header and source files -file(GLOB srcs "*.cpp") -file(GLOB hdrs "*.h") - -set(dart_collision_hdrs ${dart_collision_hdrs} ${hdrs} PARENT_SCOPE) -set(dart_collision_srcs ${dart_collision_srcs} ${srcs} PARENT_SCOPE) - -# Library -#dart_add_library(dart_collision_fcl_mesh ${srcs} ${hdrs}) -#target_link_libraries(dart_collision_fcl_mesh ${DART_CORE_DEPENDENCIES}) - -# Generate header for this namespace -dart_get_filename_components(header_names "collision_fcl_mesh headers" ${hdrs}) -dart_generate_include_header_list( - collision_fcl_mesh_headers - "dart/collision/fcl_mesh/" - "collision_fcl_mesh headers" - ${header_names} -) -configure_file( - ${CMAKE_CURRENT_SOURCE_DIR}/fcl_mesh.h.in - ${CMAKE_CURRENT_BINARY_DIR}/fcl_mesh.h -) - -# Install -install( - FILES ${hdrs} ${CMAKE_CURRENT_BINARY_DIR}/fcl_mesh.h - DESTINATION include/dart/collision/fcl_mesh - COMPONENT headers -) -#install(TARGETS dart_collision_fcl_mesh EXPORT DARTCoreTargets DESTINATION lib) -#install(TARGETS dart_collision_fcl_mesh EXPORT DARTTargets DESTINATION lib) - diff --git a/dart/collision/fcl_mesh/fcl_mesh.h.in b/dart/collision/fcl_mesh/fcl_mesh.h.in deleted file mode 100644 index adf605c8a3223..0000000000000 --- a/dart/collision/fcl_mesh/fcl_mesh.h.in +++ /dev/null @@ -1,3 +0,0 @@ -// Automatically generated file by cmake - -${collision_fcl_mesh_headers} diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index d04568b1a0e80..a8b0c1ceced40 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -39,7 +39,7 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/fcl/FCLMeshCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index a5bb23ff3bd03..f7e3c411de163 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -47,7 +47,7 @@ #endif #include "dart/collision/dart/DARTCollisionDetector.h" #include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/fcl/FCLMeshCollisionDetector.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/SoftBodyNode.h" diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 006b9d5ede77f..dc09f0fe35edb 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -46,7 +46,7 @@ #include "dart/math/math.h" #include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/collision/fcl/FCLMeshCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" From d094bd74cecd59208c3c776355b9762f12e2069b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 1 Mar 2016 22:10:17 -0500 Subject: [PATCH 14/67] Implementing filtering for collision detection --- dart/collision/CollisionGroup.cpp | 15 +--- dart/collision/CollisionGroup.h | 12 --- dart/collision/CollisionObject.h | 1 - dart/collision/Option.cpp | 7 +- dart/collision/Option.h | 9 ++- .../bullet/BulletCollisionDetector.cpp | 81 ++++++++++++++++++- .../bullet/BulletCollisionDetector.h | 10 +++ .../bullet/BulletCollisionGroupData.cpp | 42 +--------- .../bullet/BulletCollisionGroupData.h | 10 --- dart/collision/fcl/FCLCollisionDetector.cpp | 78 +++++++++++------- dart/collision/fcl/FCLCollisionDetector.h | 12 ++- .../fcl/FCLMeshCollisionDetector.cpp | 63 +++++++++++---- dart/collision/fcl/FCLMeshCollisionDetector.h | 11 +++ dart/constraint/ConstraintSolver.cpp | 7 +- dart/constraint/ConstraintSolver.h | 3 + dart/dynamics/CollisionDetector.cpp | 45 +++++++++++ dart/dynamics/CollisionDetector.h | 10 +++ 17 files changed, 283 insertions(+), 133 deletions(-) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index b60d1db03bd6e..493977dc9a27c 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -52,8 +52,7 @@ CollisionGroup::CollisionGroup( : mCollisionDetector(collisionDetector), mCollisionObjects(collObjects), mEngineData(mCollisionDetector->createCollisionGroupData( - this, mCollisionObjects).release()), - mCollisionFilter(nullptr) + this, mCollisionObjects).release()) { assert(mCollisionDetector); } @@ -259,17 +258,5 @@ void CollisionGroup::updateEngineData() mEngineData->update(); } -//============================================================================== -CollisionFilter* CollisionGroup::getCollisionFilter() -{ - return mCollisionFilter.get(); -} - -//============================================================================== -const CollisionFilter* CollisionGroup::getCollisionFilter() const -{ - return mCollisionFilter.get(); -} - } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 72ad8ce85ece0..0bd1c347c4414 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -46,9 +46,6 @@ namespace dart { namespace collision { -class CollisionDetector; -class CollisionFilter; - class CollisionGroup { public: @@ -137,12 +134,6 @@ class CollisionGroup /// detection is performed by the engine in most cases. void updateEngineData(); - /// Return collision filter - CollisionFilter* getCollisionFilter(); - - /// Return (const) collision filter - const CollisionFilter* getCollisionFilter() const; - protected: /// Collision detector @@ -154,9 +145,6 @@ class CollisionGroup /// Engine specific data std::unique_ptr mEngineData; - /// Collision filter - std::unique_ptr mCollisionFilter; - }; } // namespace collision diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index e92fe9a4a17b5..9242d4e486bf8 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -83,7 +83,6 @@ class CollisionObject /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void updateEngineData(); - // TODO(JS): remove. instead, CollisionObjectData should be updated by engine protected: diff --git a/dart/collision/Option.cpp b/dart/collision/Option.cpp index 3649a451c511d..20015569b6d9e 100644 --- a/dart/collision/Option.cpp +++ b/dart/collision/Option.cpp @@ -40,9 +40,12 @@ namespace dart { namespace collision { //============================================================================== -Option::Option(bool enableContact, size_t maxNumContacts) +Option::Option(bool enableContact, + size_t maxNumContacts, + const std::shared_ptr& collisionFilter) : enableContact(enableContact), - maxNumContacts(maxNumContacts) + maxNumContacts(maxNumContacts), + collisionFilter(collisionFilter) { // Do nothing } diff --git a/dart/collision/Option.h b/dart/collision/Option.h index 5d8835cc6463f..a1c4c05edad18 100644 --- a/dart/collision/Option.h +++ b/dart/collision/Option.h @@ -38,10 +38,13 @@ #define DART_COLLISION_OPTION_H_ #include +#include namespace dart { namespace collision { +class CollisionFilter; + struct Option { /// Flag whether compute contact information such as point, normal, and @@ -55,9 +58,13 @@ struct Option /// Maximum number of contacts to detect size_t maxNumContacts; + /// CollisionFilter + std::shared_ptr collisionFilter; + /// Constructor Option(bool enableContact = true, - size_t maxNumContacts = 100); + size_t maxNumContacts = 100, + const std::shared_ptr& collisionFilter = nullptr); }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 23bb619431a47..b12e477577e7c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -56,6 +56,55 @@ namespace collision { namespace { +struct BulletOverlapFilterCallback : public btOverlapFilterCallback +{ + BulletOverlapFilterCallback(CollisionFilter* filter) + : mFilter(filter) + { + // Do nothing + } + + // return true when pairs need collision + bool needBroadphaseCollision(btBroadphaseProxy* proxy0, + btBroadphaseProxy* proxy1) const override + { + assert((proxy0 != nullptr && proxy1 != nullptr) && + "Bullet broadphase overlapping pair proxies are nullptr"); + + bool collide = (proxy0->m_collisionFilterGroup & + proxy1->m_collisionFilterMask) != 0; + collide = collide && (proxy1->m_collisionFilterGroup & + proxy0->m_collisionFilterMask); + + if (collide && mFilter) + { + auto bulletCollObj0 + = static_cast(proxy0->m_clientObject); + auto bulletCollObj1 + = static_cast(proxy1->m_clientObject); + + auto userData0 = static_cast( + bulletCollObj0->getUserPointer()); + auto userData1 = static_cast( + bulletCollObj1->getUserPointer()); + + auto collisionDetector = userData0->collisionDetector; + assert(collisionDetector == userData1->collisionDetector); + + auto castedCD = static_cast(collisionDetector); + + auto collObj0 = castedCD->findCollisionObject(bulletCollObj0); + auto collObj1 = castedCD->findCollisionObject(bulletCollObj1); + + collide = mFilter->needCollision(collObj0, collObj1); + } + + return collide; + } + + CollisionFilter* mFilter; +}; + struct BulletContactResultCallback : btCollisionWorld::ContactResultCallback { BulletContactResultCallback(Result& result); @@ -103,6 +152,29 @@ const std::string& BulletCollisionDetector::getType() const return getTypeStatic(); } +//============================================================================== +BulletCollisionObjectData* BulletCollisionDetector::findCollisionObjectData( + btCollisionObject* bulletCollObj) const +{ + auto search = mCollisionObjectMap.find(bulletCollObj); + if (mCollisionObjectMap.end() != search) + return search->second; + else + return nullptr; +} + +//============================================================================== +CollisionObject* BulletCollisionDetector::findCollisionObject( + btCollisionObject* bulletCollObj) const +{ + auto data = findCollisionObjectData(bulletCollObj); + + if (data) + return data->getCollisionObject(); + else + return nullptr; +} + //============================================================================== std::unique_ptr BulletCollisionDetector::createCollisionObjectData( @@ -271,7 +343,7 @@ bool BulletCollisionDetector::detect( //============================================================================== bool BulletCollisionDetector::detect( CollisionGroupData* groupData, - const Option& /*option*/, Result& result) + const Option& option, Result& result) { result.contacts.clear(); @@ -282,8 +354,11 @@ bool BulletCollisionDetector::detect( auto castedData = static_cast(groupData); auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); -// auto bulletPairCache = bulletCollisionWorld->getPairCache(); -// bulletPairCache->setOverlapFilterCallback(); + auto bulletPairCache = bulletCollisionWorld->getPairCache(); + + auto filterCallback + = new BulletOverlapFilterCallback(option.collisionFilter.get()); + bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index c1ccfdbaaabeb..3890927140b98 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -46,6 +46,7 @@ namespace dart { namespace collision { class CollisionGroup; +class BulletCollisionObjectData; class BulletCollisionDetector : public CollisionDetector { @@ -66,6 +67,12 @@ class BulletCollisionDetector : public CollisionDetector using CollisionDetector::detect; + BulletCollisionObjectData* findCollisionObjectData( + btCollisionObject* bulletCollObj) const; + + CollisionObject* findCollisionObject( + btCollisionObject* bulletCollObj) const; + protected: /// Constructor @@ -123,6 +130,9 @@ class BulletCollisionDetector : public CollisionDetector std::map mShapeMap; + std::map mCollisionObjectMap; + std::shared_ptr mBulletCollisionGroupForSinglePair; }; diff --git a/dart/collision/bullet/BulletCollisionGroupData.cpp b/dart/collision/bullet/BulletCollisionGroupData.cpp index e110fd91f5803..c9e8ac657a326 100644 --- a/dart/collision/bullet/BulletCollisionGroupData.cpp +++ b/dart/collision/bullet/BulletCollisionGroupData.cpp @@ -59,11 +59,6 @@ BulletCollisionGroupData::BulletCollisionGroupData( mBulletProadphaseAlg.get(), mBulletCollisionConfiguration.get())) { - btOverlapFilterCallback* filterCallback = new CollisionFilter(); - btOverlappingPairCache* pairCache = mBulletCollisionWorld->getPairCache(); - assert(pairCache != nullptr); - pairCache->setOverlapFilterCallback(filterCallback); - addCollisionObjects(collObjects, true); } @@ -73,7 +68,8 @@ std::unique_ptr BulletCollisionGroupData::clone( const CollisionObjectPtrs& collObjects) const { return std::unique_ptr( - new BulletCollisionGroupData(mCollisionDetector, newParent, collObjects)); + new BulletCollisionGroupData(mCollisionDetector, newParent, + collObjects)); } //============================================================================== @@ -152,39 +148,5 @@ btCollisionWorld* BulletCollisionGroupData::getBulletCollisionWorld() const return mBulletCollisionWorld.get(); } -//============================================================================== -bool BulletCollisionGroupData::CollisionFilter::needBroadphaseCollision( - btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const -{ - assert((proxy0 != nullptr && proxy1 != nullptr) && - "Bullet broadphase overlapping pair proxies are nullptr"); - - bool collide = (proxy0->m_collisionFilterGroup & - proxy1->m_collisionFilterMask) != 0; - collide = collide && (proxy1->m_collisionFilterGroup & - proxy0->m_collisionFilterMask); - - if (collide) - { -// auto collObj0 = static_cast(proxy0->m_clientObject); -// auto collObj1 = static_cast(proxy1->m_clientObject); - -// auto userData0 = static_cast(collObj0->getUserPointer()); -// auto userData1 = static_cast(collObj1->getUserPointer()); - -// // Assume single collision detector -// assert(userData0->collisionDetector == userData1->collisionDetector); - -// auto collGroup0 = userData0->group; -// auto collGroup1 = userData1->group; - -// if (!collGroup0 || !) - - // TODO(JS): check collision pair if they are enabled. - } - - return collide; -} - } // namespace collision } // namespace dart diff --git a/dart/collision/bullet/BulletCollisionGroupData.h b/dart/collision/bullet/BulletCollisionGroupData.h index 9906b91bf00f9..09559f31ccef9 100644 --- a/dart/collision/bullet/BulletCollisionGroupData.h +++ b/dart/collision/bullet/BulletCollisionGroupData.h @@ -91,19 +91,9 @@ class BulletCollisionGroupData : public CollisionGroupData protected: - struct CollisionFilter : public btOverlapFilterCallback - { - // return true when pairs need collision - bool needBroadphaseCollision(btBroadphaseProxy* proxy0, - btBroadphaseProxy* proxy1) const override; - }; - /// Bullet broad-phase algorithm std::unique_ptr mBulletProadphaseAlg; - /// Bullet collision filter - std::unique_ptr mBulletDefaultCollisionFilter; - /// Bullet collision configuration std::unique_ptr mBulletCollisionConfiguration; diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 590938528c2f6..78aedb837df28 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -85,10 +85,8 @@ Contact convertContact(const fcl::Contact& fclContact, /// collision algorithm. struct FCLCollisionCallbackData { - CollisionGroup* mCollisionGroup; - - /// Collision filter - CollisionFilter* mFilter; + /// Collision detector + FCLCollisionDetector* mFclCollisionDetector; /// FCL collision request fcl::CollisionRequest mFclRequest; @@ -107,12 +105,10 @@ struct FCLCollisionCallbackData /// Constructor FCLCollisionCallbackData( -// CollisionGroup* collisionGroup, -// CollisionFilter* filter, + FCLCollisionDetector* collisionDetector, const Option* option = nullptr, Result* result = nullptr) - : /*mCollisionGroup(collisionGroup), - mFilter(filter),*/ + : mFclCollisionDetector(collisionDetector), mOption(option), mResult(result), done(false) @@ -671,6 +667,7 @@ std::shared_ptr FCLCollisionDetector::create() FCLCollisionDetector::~FCLCollisionDetector() { assert(mShapeMap.empty()); + assert(mCollisionObjectMap.empty()); } //============================================================================== @@ -686,6 +683,29 @@ const std::string& FCLCollisionDetector::getType() const return FCLCollisionDetector::getTypeStatic(); } +//============================================================================== +FCLCollisionObjectData* FCLCollisionDetector::findCollisionObjectData( + fcl::CollisionObject* fclCollObj) const +{ + auto search = mCollisionObjectMap.find(fclCollObj); + if (mCollisionObjectMap.end() != search) + return search->second; + else + return nullptr; +} + +//============================================================================== +CollisionObject* FCLCollisionDetector::findCollisionObject( + fcl::CollisionObject* fclCollObj) const +{ + auto data = findCollisionObjectData(fclCollObj); + + if (data) + return data->getCollisionObject(); + else + return nullptr; +} + //============================================================================== std::unique_ptr FCLCollisionDetector::createCollisionObjectData( @@ -704,8 +724,12 @@ FCLCollisionDetector::createCollisionObjectData( mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); } - return std::unique_ptr( - new FCLCollisionObjectData(this, parent, fclCollGeom)); + auto fclCollObjData = new FCLCollisionObjectData(this, parent, fclCollGeom); + auto fclCollObj = fclCollObjData->getFCLCollisionObject(); + + mCollisionObjectMap[fclCollObj] = fclCollObjData; + + return std::unique_ptr(fclCollObjData); } //============================================================================== @@ -726,6 +750,10 @@ void FCLCollisionDetector::reclaimCollisionObjectData( if (0u == fclCollGeomAndCount.second) mShapeMap.erase(findResult); + + auto castedCollObjData + = static_cast(collisionObjectData); + mCollisionObjectMap.erase(castedCollObjData->getFCLCollisionObject()); } //============================================================================== @@ -757,7 +785,7 @@ bool FCLCollisionDetector::detect( auto fclCollObj1 = castedData1->getFCLCollisionObject(); auto fclCollObj2 = castedData2->getFCLCollisionObject(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); collisionCallback(fclCollObj1, fclCollObj2, &collData); return !result.contacts.empty(); @@ -784,7 +812,7 @@ bool FCLCollisionDetector::detect( auto fclObject = castedObjData->getFCLCollisionObject(); auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg->collide(fclObject, &collData, collisionCallback); return !result.contacts.empty(); @@ -805,7 +833,7 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg = castedData->getFCLCollisionManager(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg->collide(&collData, collisionCallback); return !result.contacts.empty(); @@ -832,19 +860,12 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); return !result.contacts.empty(); } -//============================================================================== -FCLCollisionObject* FCLCollisionDetector::findCollisionObject( - fcl::CollisionObject* /*fclCollObj*/) -{ - return nullptr; -} - namespace { @@ -858,7 +879,8 @@ bool collisionCallback( const auto& fclRequest = collData->mFclRequest; auto& fclResult = collData->mFclResult; auto& result = *(collData->mResult); - auto filter = collData->mFilter; + auto option = collData->mOption; + auto filter = option->collisionFilter; if (collData->done) return true; @@ -866,15 +888,13 @@ bool collisionCallback( // Filtering if (filter) { -// auto collisionGroup = collData->mCollisionGroup; -// auto collisionGroupData -// = static_cast(collisionGroup->getEngineData()); + auto collisionDetector = collData->mFclCollisionDetector; -// auto collObj1 = collisionGroupData->findCollisionObject(o1); -// auto collObj2 = collisionGroupData->findCollisionObject(o2); + auto collObj1 = collisionDetector->findCollisionObject(o1); + auto collObj2 = collisionDetector->findCollisionObject(o2); -// if (filter->needCollision(collObj1, collObj2)) -// return collData->done; + if (filter->needCollision(collObj1, collObj2)) + return collData->done; } // Clear previous results diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 8fca7e3df359c..64e89369a837e 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -45,7 +45,7 @@ namespace dart { namespace collision { -class FCLCollisionObject; +class FCLCollisionObjectData; class FCLCollisionDetector : public CollisionDetector { @@ -64,6 +64,12 @@ class FCLCollisionDetector : public CollisionDetector using CollisionDetector::detect; + FCLCollisionObjectData* findCollisionObjectData( + fcl::CollisionObject* fclCollObj) const; + + CollisionObject* findCollisionObject( + fcl::CollisionObject* fclCollObj) const; + protected: /// Constructor @@ -99,8 +105,6 @@ class FCLCollisionDetector : public CollisionDetector bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) override; - FCLCollisionObject* findCollisionObject(fcl::CollisionObject* fclCollObj); - protected: using ShapeMapValue @@ -108,7 +112,7 @@ class FCLCollisionDetector : public CollisionDetector std::map mShapeMap; - std::map mFCLObjectMap; + std::map mCollisionObjectMap; }; diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp index e9935de31b234..02dd1c019ec9d 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl/FCLMeshCollisionDetector.cpp @@ -86,6 +86,9 @@ void convertOption(const Option& option, fcl::CollisionRequest& fclRequest); /// collision algorithm. struct FCLCollisionCallbackData { + /// Collision detector + FCLMeshCollisionDetector* mFclCollisionDetector; + /// FCL collision request fcl::CollisionRequest mFclRequest; @@ -103,9 +106,11 @@ struct FCLCollisionCallbackData /// Constructor FCLCollisionCallbackData( + FCLMeshCollisionDetector* collisionDetector, const Option* option = nullptr, Result* result = nullptr) - : mOption(option), + : mFclCollisionDetector(collisionDetector), + mOption(option), mResult(result), done(false) { @@ -657,6 +662,29 @@ const std::string& FCLMeshCollisionDetector::getType() const return getTypeStatic(); } +//============================================================================== +FCLMeshCollisionObjectData* FCLMeshCollisionDetector::findCollisionObjectData( + fcl::CollisionObject* fclCollObj) const +{ + auto search = mCollisionObjectMap.find(fclCollObj); + if (mCollisionObjectMap.end() != search) + return search->second; + else + return nullptr; +} + +//============================================================================== +CollisionObject* FCLMeshCollisionDetector::findCollisionObject( + fcl::CollisionObject* fclCollObj) const +{ + auto data = findCollisionObjectData(fclCollObj); + + if (data) + return data->getCollisionObject(); + else + return nullptr; +} + //============================================================================== const std::string& FCLMeshCollisionDetector::getTypeStatic() { @@ -734,7 +762,7 @@ bool FCLMeshCollisionDetector::detect( auto fclCollObj1 = castedData1->getFCLCollisionObject(); auto fclCollObj2 = castedData2->getFCLCollisionObject(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); collisionCallback(fclCollObj1, fclCollObj2, &collData); return !result.contacts.empty(); @@ -759,7 +787,7 @@ bool FCLMeshCollisionDetector::detect( auto fclObject = castedObjData->getFCLCollisionObject(); auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg->collide(fclObject, &collData, collisionCallback); return !result.contacts.empty(); @@ -779,7 +807,7 @@ bool FCLMeshCollisionDetector::detect( auto broadPhaseAlg = castedData->getFCLCollisionManager(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg->collide(&collData, collisionCallback); return !result.contacts.empty(); @@ -804,7 +832,7 @@ bool FCLMeshCollisionDetector::detect( auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); - FCLCollisionCallbackData collData(&option, &result); + FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); return !result.contacts.empty(); @@ -819,21 +847,28 @@ bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { - FCLCollisionCallbackData* collData = static_cast(cdata); + auto collData = static_cast(cdata); - const fcl::CollisionRequest& fclRequest = collData->mFclRequest; - fcl::CollisionResult& fclResult = collData->mFclResult; - Result& result = *(collData->mResult); -// FCLEngine* cd = collData->collisionDetector; - // TODO(JS): take filter object instead of collision detector + const auto& fclRequest = collData->mFclRequest; + auto& fclResult = collData->mFclResult; + auto& result = *(collData->mResult); + auto option = collData->mOption; + auto filter = option->collisionFilter; if (collData->done) return true; // Filtering -// if (!cd->isCollidable(cd->findCollisionNode(o1), cd->findCollisionNode(o2))) -// return collData->done; - // TODO(JS): disabled until other functionalities are implemented + if (filter) + { + auto collisionDetector = collData->mFclCollisionDetector; + + auto collObj1 = collisionDetector->findCollisionObject(o1); + auto collObj2 = collisionDetector->findCollisionObject(o2); + + if (filter->needCollision(collObj1, collObj2)) + return collData->done; + } // Clear previous results fclResult.clear(); diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.h b/dart/collision/fcl/FCLMeshCollisionDetector.h index 415cdc6c7cc5e..1fce89501b61a 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.h +++ b/dart/collision/fcl/FCLMeshCollisionDetector.h @@ -44,6 +44,8 @@ namespace dart { namespace collision { +class FCLMeshCollisionObjectData; + class FCLMeshCollisionDetector : public CollisionDetector { public: @@ -61,6 +63,12 @@ class FCLMeshCollisionDetector : public CollisionDetector using CollisionDetector::detect; + FCLMeshCollisionObjectData* findCollisionObjectData( + fcl::CollisionObject* fclCollObj) const; + + CollisionObject* findCollisionObject( + fcl::CollisionObject* fclCollObj) const; + protected: /// Constructor @@ -103,6 +111,9 @@ class FCLMeshCollisionDetector : public CollisionDetector std::map mShapeMap; + std::map mCollisionObjectMap; + }; } // namespace collision diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index a8b0c1ceced40..7e6ee62b8d8d5 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -67,6 +67,9 @@ using namespace dynamics; ConstraintSolver::ConstraintSolver(double _timeStep) : mCollisionDetector(collision::FCLCollisionDetector::create()), mCollisionGroup(mCollisionDetector->createCollisionGroup()), + mCollisionOption(collision::Option(true, 100, + std::unique_ptr( + new BodyNodeCollisionFilter()))), mTimeStep(_timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { @@ -354,9 +357,7 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- mCollisionResult.contacts.clear(); - collision::Option option; - option.enableContact = true; - mCollisionGroup->detect(option, mCollisionResult); + mCollisionGroup->detect(mCollisionOption, mCollisionResult); // Destroy previous contact constraints mContactConstraints.clear(); diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index 8ec5a323c3cdd..01b8b64b97dd8 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -169,6 +169,9 @@ class ConstraintSolver /// Collision group std::shared_ptr mCollisionGroup; + /// Last collision checking result + collision::Option mCollisionOption; + /// Last collision checking result collision::Result mCollisionResult; diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index 95d7699867550..1d57b1054df1f 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -41,6 +41,51 @@ namespace dart { namespace dynamics { +//============================================================================== +bool BodyNodeCollisionFilter::needCollision( + const collision::CollisionObject* object1, + const collision::CollisionObject* object2) const +{ + auto castedObj1 = static_cast(object1); + auto castedObj2 = static_cast(object2); + + auto bodyNode1 = castedObj1->getBodyNode(); + auto bodyNode2 = castedObj2->getBodyNode(); + + if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable()) + return false; + + if (bodyNode1->getSkeleton() == bodyNode2->getSkeleton()) + { + auto skeleton = bodyNode1->getSkeleton(); + + if (!skeleton->isEnabledSelfCollisionCheck()) + return false; + + if (!skeleton->isEnabledAdjacentBodyCheck()) + { + if (isAdjacentBodies(bodyNode1, bodyNode2)) + return false; + } + } + + return true; +} + +//============================================================================== +bool BodyNodeCollisionFilter::isAdjacentBodies(const BodyNode* bodyNode1, + const BodyNode* bodyNode2) const +{ + if ((bodyNode1->getParentBodyNode() == bodyNode2) + || (bodyNode2->getParentBodyNode() == bodyNode1)) + { + assert(bodyNode1->getSkeleton() == bodyNode2->getSkeleton()); + return true; + } + + return false; +} + //============================================================================== ShapeFrameCollisionObject::ShapeFrameCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index 92f46bd9ec116..20c1e6ae7ee2a 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -40,6 +40,7 @@ #include #include "dart/collision/CollisionDetector.h" +#include "dart/collision/CollisionFilter.h" #include "dart/collision/fcl/FCLCollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/dynamics/SmartPointer.h" @@ -47,6 +48,15 @@ namespace dart { namespace dynamics { +struct BodyNodeCollisionFilter : collision::CollisionFilter +{ + bool needCollision(const collision::CollisionObject* object1, + const collision::CollisionObject* object2) const override; + + bool isAdjacentBodies(const BodyNode* bodyNode1, + const BodyNode* bodyNode2) const; +}; + class ShapeFrameCollisionObject : public collision::CollisionObject { public: From e330676dfb74588ac9f173227e34ef367f931cf9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 4 Mar 2016 10:56:24 -0500 Subject: [PATCH 15/67] Improve comparaing method for two collision objects --- dart/collision/CollisionGroup.cpp | 22 ++++++++++++++---- dart/collision/CollisionObject.cpp | 6 +++++ dart/collision/CollisionObject.h | 6 +++++ .../bullet/BulletCollisionDetector.cpp | 3 +-- dart/collision/fcl/FCLCollisionDetector.cpp | 5 ++-- .../fcl/FCLMeshCollisionDetector.cpp | 20 ++++++++++++---- dart/dynamics/CollisionDetector.cpp | 23 +++++++++++++++++++ dart/dynamics/CollisionDetector.h | 3 +++ unittests/testCollision.cpp | 23 +++++++++++++++++++ 9 files changed, 98 insertions(+), 13 deletions(-) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 493977dc9a27c..9107cd6ca052b 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -106,12 +106,27 @@ CollisionDetector* CollisionGroup::getCollisionDetector() const return mCollisionDetector.get(); } +//============================================================================== +struct is_collision_object_equal + : public std::unary_function +{ + explicit is_collision_object_equal(const CollisionObjectPtr& object) + : mObject(object) {} + + bool operator() (const CollisionObjectPtr& arg) + { + return mObject->isEqual(arg.get()); + } + + const CollisionObjectPtr& mObject; +}; + //============================================================================== bool CollisionGroup::hasCollisionObject( const CollisionGroup::CollisionObjectPtr& object) const { - return std::find(mCollisionObjects.begin(), - mCollisionObjects.end(), object) + return std::find_if(mCollisionObjects.begin(), mCollisionObjects.end(), + is_collision_object_equal(object)) != mCollisionObjects.end(); } @@ -120,10 +135,7 @@ void CollisionGroup::addCollisionObject( const CollisionObjectPtr& object, bool init) { if (hasCollisionObject(object)) - { - // TODO(JS): print warning message return; - } object->addGroup(this); diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 6f856dbbe500f..ff0d7e84b0478 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -166,5 +166,11 @@ const Eigen::Isometry3d FreeCollisionObject::getTransform() const return mW; } +//============================================================================== +bool FreeCollisionObject::isEqual(const CollisionObject* other) +{ + return this == other; +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 9242d4e486bf8..f68889a14a562 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -64,6 +64,9 @@ class CollisionObject /// Return the transformation of this CollisionObject in world coordinates virtual const Eigen::Isometry3d getTransform() const = 0; + /// Return true if this CollisionObject is identical to other + virtual bool isEqual(const CollisionObject* other) = 0; + /// Perform collision detection with other CollisionObject. /// /// Return false if the engine type of the other CollisionObject is different @@ -141,6 +144,9 @@ class FreeCollisionObject : public CollisionObject /// Return world transformation of this FreeCollisionObject const Eigen::Isometry3d getTransform() const override; + // Documentation inherited + bool isEqual(const CollisionObject* other) override; + protected: /// Transformation in world coordinates diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index b12e477577e7c..f31ca0a0b58eb 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -109,8 +109,6 @@ struct BulletContactResultCallback : btCollisionWorld::ContactResultCallback { BulletContactResultCallback(Result& result); -// bool needsCollision(btBroadphaseProxy* proxy0) const override; - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, @@ -188,6 +186,7 @@ BulletCollisionDetector::createCollisionObjectData( ShapeMapValue& pair = findResult->second; BulletCollsionPack& pack = pair.first; bulletCollGeom = pack.collisionShape.get(); + pair.second++; } else { diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 78aedb837df28..ded414e9f1c5b 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -717,6 +717,7 @@ FCLCollisionDetector::createCollisionObjectData( if (mShapeMap.end() != findResult) { fclCollGeom = findResult->second.first; + findResult->second.second++; } else { @@ -743,7 +744,7 @@ void FCLCollisionDetector::reclaimCollisionObjectData( auto findResult = mShapeMap.find(shape); assert(mShapeMap.end() != findResult); - auto fclCollGeomAndCount = findResult->second; + auto& fclCollGeomAndCount = findResult->second; assert(0u != fclCollGeomAndCount.second); fclCollGeomAndCount.second--; @@ -893,7 +894,7 @@ bool collisionCallback( auto collObj1 = collisionDetector->findCollisionObject(o1); auto collObj2 = collisionDetector->findCollisionObject(o2); - if (filter->needCollision(collObj1, collObj2)) + if (!filter->needCollision(collObj1, collObj2)) return collData->done; } diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp index 02dd1c019ec9d..37a79097a583b 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl/FCLMeshCollisionDetector.cpp @@ -704,6 +704,7 @@ FCLMeshCollisionDetector::createCollisionObjectData( if (mShapeMap.end() != findResult) { fclCollGeom = findResult->second.first; + findResult->second.second++; } else { @@ -711,8 +712,13 @@ FCLMeshCollisionDetector::createCollisionObjectData( mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); } - return std::unique_ptr( - new FCLMeshCollisionObjectData(this, parent, fclCollGeom)); + auto fclCollObjData + = new FCLMeshCollisionObjectData(this, parent, fclCollGeom); + auto fclCollObj = fclCollObjData->getFCLCollisionObject(); + + mCollisionObjectMap[fclCollObj] = fclCollObjData; + + return std::unique_ptr(fclCollObjData); } //============================================================================== @@ -726,13 +732,17 @@ void FCLMeshCollisionDetector::reclaimCollisionObjectData( auto findResult = mShapeMap.find(shape); assert(mShapeMap.end() != findResult); - auto fclCollGeomAndCount = findResult->second; + auto& fclCollGeomAndCount = findResult->second; assert(0u != fclCollGeomAndCount.second); fclCollGeomAndCount.second--; if (0u == fclCollGeomAndCount.second) mShapeMap.erase(findResult); + + auto castedCollObjData + = static_cast(collisionObjectData); + mCollisionObjectMap.erase(castedCollObjData->getFCLCollisionObject()); } //============================================================================== @@ -865,8 +875,10 @@ bool collisionCallback(fcl::CollisionObject* o1, auto collObj1 = collisionDetector->findCollisionObject(o1); auto collObj2 = collisionDetector->findCollisionObject(o2); + assert(collObj1); + assert(collObj2); - if (filter->needCollision(collObj1, collObj2)) + if (!filter->needCollision(collObj1, collObj2)) return collData->done; } diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index 1d57b1054df1f..adcb19732443b 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -46,6 +46,9 @@ bool BodyNodeCollisionFilter::needCollision( const collision::CollisionObject* object1, const collision::CollisionObject* object2) const { + if (object1 == object2) + return false; + auto castedObj1 = static_cast(object1); auto castedObj2 = static_cast(object2); @@ -125,6 +128,26 @@ const Eigen::Isometry3d ShapeFrameCollisionObject::getTransform() const return mBodyNode->getWorldTransform() * mShape->getLocalTransform(); } +//============================================================================== +bool ShapeFrameCollisionObject::isEqual(const collision::CollisionObject* other) +{ + if (this == other) + return true; + + auto castedOther = dynamic_cast(other); + + if (!castedOther) + return false; + + if (mShape != castedOther->mShape) + return false; + + if (mBodyNode != castedOther->mBodyNode) + return false; + + return true; +} + //============================================================================== BodyNodePtr ShapeFrameCollisionObject::getBodyNode() const { diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index 20c1e6ae7ee2a..e1249d85a77a9 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -70,6 +70,9 @@ class ShapeFrameCollisionObject : public collision::CollisionObject // Documentation inherited const Eigen::Isometry3d getTransform() const override; + // Documentation inherited + bool isEqual(const CollisionObject* other) override; + /// Return BodyNode pointer associated with this ShapeFrameCollisionObject dynamics::BodyNodePtr getBodyNode() const; diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index dc09f0fe35edb..80f90b2aa8cc0 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -610,6 +610,29 @@ void testBodyNodes() EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); EXPECT_TRUE(cd->detect(obj1.get(), obj2.get(), option, result)); + + auto obj3 = cd->template createCollisionObject( + boxShape2, boxBody2); + + EXPECT_FALSE(obj1->isEqual(obj2.get())); + EXPECT_FALSE(obj2->isEqual(obj1.get())); + + EXPECT_FALSE(obj1->isEqual(obj3.get())); + EXPECT_FALSE(obj3->isEqual(obj1.get())); + + EXPECT_TRUE(obj2->isEqual(obj3.get())); + EXPECT_TRUE(obj3->isEqual(obj2.get())); + + collision::CollisionGroupPtr group1(new collision::CollisionGroup(cd)); + group1->addCollisionObject(obj2); + collision::CollisionGroupPtr group2(new collision::CollisionGroup(cd)); + group2->addCollisionObject(obj3); + + EXPECT_EQ(group1->getCollisionObjects().size(), 1u); + EXPECT_EQ(group2->getCollisionObjects().size(), 1u); + + group1->unionGroup(group2); + EXPECT_EQ(group1->getCollisionObjects().size(), 1u); } //============================================================================== From e22f4ce2b743801461704bf8e614a8388d470e3b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 11 Mar 2016 11:35:25 -0500 Subject: [PATCH 16/67] Enforcing CollisionDetector to create unique collision object for the same associated object -- not completed yet --- dart/collision/CollisionDetector.cpp | 46 +++++++++++ dart/collision/CollisionDetector.h | 29 ++++--- dart/collision/CollisionObject.cpp | 28 ++++--- dart/collision/CollisionObject.h | 20 ++--- dart/collision/detail/CollisionDetector.h | 70 ++++++++++++++++ dart/dynamics/CollisionDetector.cpp | 99 +++++++++-------------- dart/dynamics/CollisionDetector.h | 36 ++------- unittests/testCollision.cpp | 2 +- 8 files changed, 206 insertions(+), 124 deletions(-) create mode 100644 dart/collision/detail/CollisionDetector.h diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 183783d9fa4f8..7023c2be3c53e 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -50,6 +50,12 @@ namespace dart { namespace collision { +//============================================================================== +CollisionDetector::~CollisionDetector() +{ +// assert(mCollisionObjects.empty()); +} + //============================================================================== std::shared_ptr CollisionDetector::createCollisionGroup( const std::vector& objects) @@ -116,6 +122,46 @@ bool CollisionDetector::detect( option, result); } +//============================================================================== +struct is_collision_object_equal + : public std::unary_function +{ + explicit is_collision_object_equal(const CollisionObject* object) + : mObject(object) {} + + bool operator() (const WeakCollisionObjectPtr& arg) + { + auto locked = arg.lock(); + assert(locked); + + return mObject == locked.get(); + } + + const CollisionObject* mObject; +}; + +//============================================================================== +void CollisionDetector::reclaimCollisionObject(CollisionObject* collisionObject) +{ + assert(hasCollisionObject(collisionObject)); + + mCollisionObjects.erase( + std::remove_if(mCollisionObjects.begin(), mCollisionObjects.end(), + is_collision_object_equal(collisionObject)), + mCollisionObjects.end()); + + reclaimCollisionObjectData(collisionObject->getEngineData()); +} + +//============================================================================== +bool CollisionDetector::hasCollisionObject( + const CollisionObject* collisionObject) const +{ + return std::find_if(mCollisionObjects.begin(), mCollisionObjects.end(), + is_collision_object_equal(collisionObject)) + != mCollisionObjects.end(); +} + //============================================================================== bool CollisionDetector::detect( CollisionGroupData* group, CollisionObjectData* object, diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index b246691b65092..1c9ed2946c313 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -61,6 +61,9 @@ class CollisionDetector : public std::enable_shared_from_this friend class CollisionObject; friend class CollisionGroup; + /// Destructor + virtual ~CollisionDetector(); + /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; @@ -102,12 +105,15 @@ class CollisionDetector : public std::enable_shared_from_this /// Constructor CollisionDetector() = default; + /// Reclaim collisionObject from this CollisionDetector. + void reclaimCollisionObject(CollisionObject* collisionObject); + /// Create collision detection engine specific data for CollisionObject virtual std::unique_ptr createCollisionObjectData( CollisionObject* parent, const dynamics::ShapePtr& shape) = 0; - /// Create collision detection engine specific data for CollisionObject + /// Reclaim collision detection engine specific data for CollisionObject virtual void reclaimCollisionObjectData( CollisionObjectData* collisionObjectData) = 0; @@ -116,6 +122,9 @@ class CollisionDetector : public std::enable_shared_from_this CollisionGroup* parent, const CollisionObjectPtrs& collObjects) = 0; + /// Return true if collisionObject is created from this CollisionDetector + bool hasCollisionObject(const CollisionObject* collisionObject) const; + /// Perform collision detection for object1-object2. virtual bool detect(CollisionObjectData* object1, CollisionObjectData* object2, @@ -137,20 +146,16 @@ class CollisionDetector : public std::enable_shared_from_this virtual bool detect(CollisionGroupData* group1, CollisionGroupData* group2, const Option& option, Result& result) = 0; -}; +protected: -//============================================================================== -template -std::shared_ptr -CollisionDetector::createCollisionObject( - const dynamics::ShapePtr& shape, - const Args&... args) -{ - return std::make_shared( - shared_from_this(), shape, args...); -} + /// CollisionObject array created from this CollisionDetector + std::vector mCollisionObjects; + +}; } // namespace collision } // namespace dart +#include "dart/collision/detail/CollisionDetector.h" + #endif // DART_COLLISION_COLLISIONDETECTOR_H_ diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index ff0d7e84b0478..d7b985da6e28b 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -45,6 +45,7 @@ namespace collision { //============================================================================== CollisionObject::~CollisionObject() { +// mCollisionDetector->reclaimCollisionObject(this); mCollisionDetector->reclaimCollisionObjectData(mEngineData.get()); } @@ -128,18 +129,8 @@ std::shared_ptr FreeCollisionObject::create( const CollisionDetectorPtr& collisionDetector, const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf) { - return std::make_shared(collisionDetector, shape, tf); -} - -//============================================================================== -FreeCollisionObject::FreeCollisionObject( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf) - : CollisionObject(collisionDetector, shape), - mW(tf) -{ - // Do nothing + return collisionDetector->createCollisionObject( + shape, tf); } //============================================================================== @@ -167,10 +158,21 @@ const Eigen::Isometry3d FreeCollisionObject::getTransform() const } //============================================================================== -bool FreeCollisionObject::isEqual(const CollisionObject* other) +bool FreeCollisionObject::isEqual(const CollisionObject* other) const { return this == other; } +//============================================================================== +FreeCollisionObject::FreeCollisionObject( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapePtr& shape, + const Eigen::Isometry3d& tf) + : CollisionObject(collisionDetector, shape), + mW(tf) +{ + // Do nothing +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index f68889a14a562..3b223cc51d960 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -65,7 +65,7 @@ class CollisionObject virtual const Eigen::Isometry3d getTransform() const = 0; /// Return true if this CollisionObject is identical to other - virtual bool isEqual(const CollisionObject* other) = 0; + virtual bool isEqual(const CollisionObject* other) const = 0; /// Perform collision detection with other CollisionObject. /// @@ -120,17 +120,12 @@ class FreeCollisionObject : public CollisionObject { public: - static std::shared_ptr create( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); + friend class CollisionDetector; - /// Constructor - FreeCollisionObject( + static std::shared_ptr create( const CollisionDetectorPtr& collisionDetector, const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); - // TODO(JS): change to engine pointer /// Set world transformation of this FreeCollisionObject void setTransform(const Eigen::Isometry3d& tf); @@ -145,10 +140,17 @@ class FreeCollisionObject : public CollisionObject const Eigen::Isometry3d getTransform() const override; // Documentation inherited - bool isEqual(const CollisionObject* other) override; + bool isEqual(const CollisionObject* other) const override; protected: + /// Constructor + FreeCollisionObject( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapePtr& shape, + const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); + // TODO(JS): change to engine pointer + /// Transformation in world coordinates Eigen::Isometry3d mW; diff --git a/dart/collision/detail/CollisionDetector.h b/dart/collision/detail/CollisionDetector.h new file mode 100644 index 0000000000000..d714aac5ccf76 --- /dev/null +++ b/dart/collision/detail/CollisionDetector.h @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ +#define DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ + +#include "dart/collision/CollisionDetector.h" + +namespace dart { +namespace collision { + +//============================================================================== +template +std::shared_ptr CollisionDetector::createCollisionObject( + const dynamics::ShapePtr& shape, const Args&... args) +{ + auto newCollisionObject = std::shared_ptr( + new CollisionObjectType(shared_from_this(), shape, args...)); + + for (auto collisionObject : mCollisionObjects) + { + auto locked = collisionObject.lock(); + assert(locked); + + if (newCollisionObject->isEqual(locked.get())) + return std::static_pointer_cast(locked); + } + + mCollisionObjects.push_back(newCollisionObject); + + return newCollisionObject; +} + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_COLLISIONDETECTOR_H_ diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index adcb19732443b..3d52067141fc2 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -89,6 +89,44 @@ bool BodyNodeCollisionFilter::isAdjacentBodies(const BodyNode* bodyNode1, return false; } +//============================================================================== +const Eigen::Isometry3d ShapeFrameCollisionObject::getTransform() const +{ + return mBodyNode->getWorldTransform() * mShape->getLocalTransform(); +} + +//============================================================================== +bool ShapeFrameCollisionObject::isEqual( + const collision::CollisionObject* other) const +{ + if (this == other) + return true; + + auto castedOther = dynamic_cast(other); + + if (!castedOther) + return false; + + if (mShape != castedOther->mShape) + return false; + + if (mBodyNode != castedOther->mBodyNode) + return false; + + // If castedOther has the same shape and body node then it must be the same + // pointer with this as well because the collision detector doesn't allow to + // create new collision objects for the same shape and body node. + assert(this != other); + + return true; +} + +//============================================================================== +BodyNodePtr ShapeFrameCollisionObject::getBodyNode() const +{ + return mBodyNode; +} + //============================================================================== ShapeFrameCollisionObject::ShapeFrameCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, @@ -122,38 +160,6 @@ ShapeFrameCollisionObject::ShapeFrameCollisionObject( } } -//============================================================================== -const Eigen::Isometry3d ShapeFrameCollisionObject::getTransform() const -{ - return mBodyNode->getWorldTransform() * mShape->getLocalTransform(); -} - -//============================================================================== -bool ShapeFrameCollisionObject::isEqual(const collision::CollisionObject* other) -{ - if (this == other) - return true; - - auto castedOther = dynamic_cast(other); - - if (!castedOther) - return false; - - if (mShape != castedOther->mShape) - return false; - - if (mBodyNode != castedOther->mBodyNode) - return false; - - return true; -} - -//============================================================================== -BodyNodePtr ShapeFrameCollisionObject::getBodyNode() const -{ - return mBodyNode; -} - //============================================================================== ShapeFrameCollisionObjectPtr createShapeFrameCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, @@ -202,34 +208,5 @@ collision::CollisionGroupPtr createShapeFrameCollisionGroup( return group; } -////============================================================================== -//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, -// const ShapePtr& shape1, const BodyNodePtr& body1, -// const ShapePtr& shape2, const BodyNodePtr& body2, -// const collision::Option& option, -// collision::Result& result) -//{ -// auto obj1 = collisionDetector->createCollisionObject(shape1, body1); -// auto obj2 = collisionDetector->createCollisionObject(shape2, body2); - -// return obj1->detect(obj2.get(), option, result); -//} - -////============================================================================== -//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, -// const SkeletonPtr& skel1, -// const SkeletonPtr& skel2, -// const collision::Option& option, -// collision::Result& result) -//{ -// auto objects1 = createShapeFrameCollisionObjects(collisionDetector, skel1); -// auto objects2 = createShapeFrameCollisionObjects(collisionDetector, skel2); - -// auto group1 = collisionDetector->createCollisionGroup(objects1); -// auto group2 = collisionDetector->createCollisionGroup(objects2); - -// return group1->detect(group2.get(), option, result); -//} - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index e1249d85a77a9..5953cfd0d1098 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -61,23 +61,25 @@ class ShapeFrameCollisionObject : public collision::CollisionObject { public: - ShapeFrameCollisionObject( - const collision::CollisionDetectorPtr& collisionDetector, - const ShapePtr& shape, - const BodyNodePtr& bodyNode); - // TODO(JS): this should be replaced by ShapeNode + friend class collision::CollisionDetector; // Documentation inherited const Eigen::Isometry3d getTransform() const override; // Documentation inherited - bool isEqual(const CollisionObject* other) override; + bool isEqual(const CollisionObject* other) const override; /// Return BodyNode pointer associated with this ShapeFrameCollisionObject dynamics::BodyNodePtr getBodyNode() const; protected: + ShapeFrameCollisionObject( + const collision::CollisionDetectorPtr& collisionDetector, + const ShapePtr& shape, + const BodyNodePtr& bodyNode); + // TODO(JS): this should be replaced by ShapeNode + dynamics::BodyNodePtr mBodyNode; // TODO(JS): this should be changed to ShapeNode @@ -101,28 +103,6 @@ collision::CollisionGroupPtr createShapeFrameCollisionGroup( const collision::CollisionDetectorPtr& collisionDetector, const dynamics::SkeletonPtr& skel); -///// Checks the collisions between two BodyNodes for one time. -//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, -// const dynamics::ShapePtr& shape1, -// const BodyNodePtr& body1, -// const dynamics::ShapePtr& shape2, -// const BodyNodePtr& body2, -// const collision::Option& option, -// collision::Result& result); - -///// Checks the collisions between two Skeletons for one time. -///// -///// This function creates two collision groups internally as local variables -///// of the skeletons for single time collision check. If you want to check -///// more than once, then create two collision groups explicitly using -///// createShapeNodeCollisionGroup() and check the collisions with the groups -///// for better performance. -//bool detect(const DynamicsCollisionDetectorPtr& collisionDetector, -// const dynamics::SkeletonPtr& skel1, -// const dynamics::SkeletonPtr& skel2, -// const collision::Option& option, -// collision::Result& result); - } // namespace dynamics } // namespace dart diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 80f90b2aa8cc0..98a379d38067a 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -525,7 +525,7 @@ void testFreeCollisionObjects() // Create collision objects in different ways auto obj1 = cd->template createCollisionObject(shape1); auto obj2 = FreeCollisionObject::create(cd, shape2); - auto obj3 = std::shared_ptr(new FreeCollisionObject(cd, shape3)); + auto obj3 = FreeCollisionObject::create(cd, shape3); collision::CollisionGroupPtr group1(new collision::CollisionGroup(cd)); group1->addCollisionObject(obj1); From 6004a1bb57a211d7c5cb21f318d886688086af65 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 11 Mar 2016 15:59:20 -0500 Subject: [PATCH 17/67] Change ShapeFrameCollisionObject to ShapeNodeCollisionObject --- .../bullet/BulletCollisionDetector.cpp | 16 ++-- dart/collision/fcl/FCLCollisionDetector.cpp | 6 +- .../fcl/FCLMeshCollisionDetector.cpp | 16 ++-- .../fcl/FCLMeshCollisionObjectData.cpp | 2 +- .../fcl/FCLMeshCollisionObjectData.h | 6 +- dart/constraint/ConstraintSolver.cpp | 12 +-- dart/constraint/ConstraintSolver.h | 2 +- dart/constraint/ContactConstraint.cpp | 8 +- dart/constraint/SoftContactConstraint.cpp | 8 +- dart/dynamics/CollisionDetector.cpp | 81 ++++++++++--------- dart/dynamics/CollisionDetector.h | 34 ++++---- tutorials/tutorialBiped-Finished.cpp | 4 +- tutorials/tutorialBiped.cpp | 4 +- unittests/testCollision.cpp | 16 ++-- 14 files changed, 115 insertions(+), 100 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 64d03b3e8e4d9..f74b1de2b6c58 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -231,8 +231,8 @@ BulletCollisionDetector::createCollisionGroupData( //============================================================================== Contact convertContact(const btManifoldPoint& bulletManifoldPoint, - const BulletCollisionObjectUserData* userData1, - const BulletCollisionObjectUserData* userData2) + const BulletCollisionObjectData::UserData* userData1, + const BulletCollisionObjectData::UserData* userData2) { assert(userData1); assert(userData2); @@ -267,8 +267,10 @@ void convertContacts(btCollisionWorld* collWorld, Result& result) auto userPointer0 = bulletCollObj0->getUserPointer(); auto userPointer1 = bulletCollObj1->getUserPointer(); - auto userDataA = static_cast(userPointer1); - auto userDataB = static_cast(userPointer0); + auto userDataA + = static_cast(userPointer1); + auto userDataB + = static_cast(userPointer0); auto numContacts = contactManifold->getNumContacts(); @@ -594,8 +596,10 @@ btScalar BulletContactResultCallback::addSingleResult( auto userPointer0 = colObj0Wrap->getCollisionObject()->getUserPointer(); auto userPointer1 = colObj1Wrap->getCollisionObject()->getUserPointer(); - auto userDataA = static_cast(userPointer1); - auto userDataB = static_cast(userPointer0); + auto userDataA + = static_cast(userPointer1); + auto userDataB + = static_cast(userPointer0); mResult.contacts.push_back(convertContact(cp, userDataA, userDataB)); diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index ded414e9f1c5b..c9510daffb3ef 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -1023,8 +1023,10 @@ Contact convertContact(const fcl::Contact& fclContact, contact.triID1 = fclContact.b1; contact.triID2 = fclContact.b2; - auto userData1 = static_cast(o1->getUserData()); - auto userData2 = static_cast(o2->getUserData()); + auto userData1 + = static_cast(o1->getUserData()); + auto userData2 + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); contact.collisionObject1 = userData1->mCollisionObject; diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp index 37a79097a583b..97f3c76aedeef 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl/FCLMeshCollisionDetector.cpp @@ -926,10 +926,10 @@ void postProcess(const fcl::CollisionResult& fclResult, Contact pair1, pair2; const fcl::Contact& c = fclResult.getContact(k); - FCLCollisionObjectUserData* userData1 - = static_cast(o1->getUserData()); - FCLCollisionObjectUserData* userData2 - = static_cast(o2->getUserData()); + auto userData1 + = static_cast(o1->getUserData()); + auto userData2 + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); @@ -1202,10 +1202,10 @@ Contact convertContact(const fcl::Contact& fclContact, contact.triID1 = fclContact.b1; contact.triID2 = fclContact.b2; - FCLCollisionObjectUserData* userData1 - = static_cast(o1->getUserData()); - FCLCollisionObjectUserData* userData2 - = static_cast(o2->getUserData()); + auto userData1 + = static_cast(o1->getUserData()); + auto userData2 + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); contact.collisionObject1 = userData1->mCollisionObject; diff --git a/dart/collision/fcl/FCLMeshCollisionObjectData.cpp b/dart/collision/fcl/FCLMeshCollisionObjectData.cpp index 760118549248f..5297066be0db4 100644 --- a/dart/collision/fcl/FCLMeshCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLMeshCollisionObjectData.cpp @@ -118,7 +118,7 @@ FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( CollisionObject* parent, const boost::shared_ptr& fclCollGeom) : CollisionObjectData(collisionDetector, parent), - mFCLCollisionObjectUserData(new FCLCollisionObjectUserData(parent)), + mFCLCollisionObjectUserData(new FCLCollisionObjectData::UserData(parent)), mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) { mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); diff --git a/dart/collision/fcl/FCLMeshCollisionObjectData.h b/dart/collision/fcl/FCLMeshCollisionObjectData.h index 1628aeaeb5576..c582d4540a59d 100644 --- a/dart/collision/fcl/FCLMeshCollisionObjectData.h +++ b/dart/collision/fcl/FCLMeshCollisionObjectData.h @@ -43,13 +43,11 @@ #include #include "dart/collision/CollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObjectData.h" namespace dart { namespace collision { -class CollisionObject; -class FCLCollisionObjectUserData; - class FCLMeshCollisionObjectData : public CollisionObjectData { public: @@ -76,7 +74,7 @@ class FCLMeshCollisionObjectData : public CollisionObjectData protected: /// FCL collision geometry user data - std::unique_ptr mFCLCollisionObjectUserData; + std::unique_ptr mFCLCollisionObjectUserData; /// FCL collision object std::unique_ptr mFCLCollisionObject; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 7e6ee62b8d8d5..163ae0e07e112 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -545,18 +545,18 @@ void ConstraintSolver::solveConstrainedGroups() //============================================================================== bool ConstraintSolver::isSoftContact(const collision::Contact& _contact) const { - assert(dynamic_cast( + assert(dynamic_cast( _contact.collisionObject1)); - assert(dynamic_cast( + assert(dynamic_cast( _contact.collisionObject2)); - auto shapeFrameCollObj1 = static_cast( + auto shapeFrameCollObj1 = static_cast( _contact.collisionObject1); - auto shapeFrameCollObj2 = static_cast( + auto shapeFrameCollObj2 = static_cast( _contact.collisionObject2); - auto bodyNode1 = shapeFrameCollObj1->getBodyNode().get(); - auto bodyNode2 = shapeFrameCollObj2->getBodyNode().get(); + auto bodyNode1 = shapeFrameCollObj1->getBodyNode(); + auto bodyNode2 = shapeFrameCollObj2->getBodyNode(); auto bodyNode1IsSoft = dynamic_cast(bodyNode1) != nullptr; diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index 01b8b64b97dd8..2568130afa575 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -50,7 +50,7 @@ namespace dart { namespace dynamics { class Skeleton; -class ShapeFrameCollisionObject; +class ShapeNodeCollisionObject; } // namespace dynamics namespace constraint { diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 79128c60bde1d..42a6ee6d5a546 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -70,9 +70,9 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, double _timeStep) : ConstraintBase(), mTimeStep(_timeStep), - mBodyNode1(static_cast( + mBodyNode1(static_cast( _contact.collisionObject1)->getBodyNode()), - mBodyNode2(static_cast( + mBodyNode2(static_cast( _contact.collisionObject2)->getBodyNode()), mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), mIsFrictionOn(true), @@ -80,9 +80,9 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, mIsBounceOn(false), mActive(false) { - assert(dynamic_cast( + assert(dynamic_cast( _contact.collisionObject1)->getBodyNode()); - assert(dynamic_cast( + assert(dynamic_cast( _contact.collisionObject2)->getBodyNode()); // TODO(JS): Assumed single contact diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index dfff4acc04e52..99ae28eea792e 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -73,9 +73,9 @@ SoftContactConstraint::SoftContactConstraint( collision::Contact& _contact, double _timeStep) : ConstraintBase(), mTimeStep(_timeStep), - mBodyNode1(static_cast( + mBodyNode1(static_cast( _contact.collisionObject1)->getBodyNode()), - mBodyNode2(static_cast( + mBodyNode2(static_cast( _contact.collisionObject2)->getBodyNode()), mSoftBodyNode1(dynamic_cast(mBodyNode1)), mSoftBodyNode2(dynamic_cast(mBodyNode2)), @@ -88,9 +88,9 @@ SoftContactConstraint::SoftContactConstraint( mIsBounceOn(false), mActive(false) { - assert(dynamic_cast( + assert(dynamic_cast( _contact.collisionObject1)->getBodyNode()); - assert(dynamic_cast( + assert(dynamic_cast( _contact.collisionObject2)->getBodyNode()); // TODO(JS): Assumed single contact diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index 3d52067141fc2..a140ebdc5a487 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -49,8 +49,8 @@ bool BodyNodeCollisionFilter::needCollision( if (object1 == object2) return false; - auto castedObj1 = static_cast(object1); - auto castedObj2 = static_cast(object2); + auto castedObj1 = static_cast(object1); + auto castedObj2 = static_cast(object2); auto bodyNode1 = castedObj1->getBodyNode(); auto bodyNode2 = castedObj2->getBodyNode(); @@ -90,19 +90,19 @@ bool BodyNodeCollisionFilter::isAdjacentBodies(const BodyNode* bodyNode1, } //============================================================================== -const Eigen::Isometry3d ShapeFrameCollisionObject::getTransform() const +const Eigen::Isometry3d ShapeNodeCollisionObject::getTransform() const { - return mBodyNode->getWorldTransform() * mShape->getLocalTransform(); + return mShapeNode->getWorldTransform(); } //============================================================================== -bool ShapeFrameCollisionObject::isEqual( +bool ShapeNodeCollisionObject::isEqual( const collision::CollisionObject* other) const { if (this == other) return true; - auto castedOther = dynamic_cast(other); + auto castedOther = dynamic_cast(other); if (!castedOther) return false; @@ -110,7 +110,7 @@ bool ShapeFrameCollisionObject::isEqual( if (mShape != castedOther->mShape) return false; - if (mBodyNode != castedOther->mBodyNode) + if (mShapeNode != castedOther->mShapeNode) return false; // If castedOther has the same shape and body node then it must be the same @@ -122,36 +122,43 @@ bool ShapeFrameCollisionObject::isEqual( } //============================================================================== -BodyNodePtr ShapeFrameCollisionObject::getBodyNode() const +ShapeNode* ShapeNodeCollisionObject::getShapeNode() { - return mBodyNode; + return mShapeNode.get(); } //============================================================================== -ShapeFrameCollisionObject::ShapeFrameCollisionObject( +const ShapeNode* ShapeNodeCollisionObject::getShapeNode() const +{ + return mShapeNode.get(); +} + +//============================================================================== +BodyNode* ShapeNodeCollisionObject::getBodyNode() +{ + return mShapeNode->getBodyNodePtr().get(); +} + +//============================================================================== +const BodyNode* ShapeNodeCollisionObject::getBodyNode() const +{ + return mShapeNode->getBodyNodePtr().get(); +} + +//============================================================================== +ShapeNodeCollisionObject::ShapeNodeCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, - const dynamics::BodyNodePtr& bodyNode) + const ShapePtr& shape, + const ShapeNodePtr& shapeNode) : collision::CollisionObject(collisionDetector, shape), - mBodyNode(bodyNode) + mShapeNode(shapeNode) { assert(collisionDetector); - assert(shape); - assert(bodyNode); + assert(shapeNode); + assert(shapeNode->getShape()); + assert(shape == shapeNode->getShape()); - auto found = false; - auto numShapes = mBodyNode->getNumCollisionShapes(); - for (auto i = 0u; i < numShapes; ++i) - { - auto shapeIt = mBodyNode->getCollisionShape(i); - if (shape == shapeIt) - { - found = true; - break; - } - } - - if (!found) + if (!mShapeNode->hasCollisionAddon()) { dtwarn << "[ShapeFrameCollisionObject::constructor] Attempting to create " << "ShapeFrameCollisionObject with invalid pair of Shape and " @@ -161,13 +168,12 @@ ShapeFrameCollisionObject::ShapeFrameCollisionObject( } //============================================================================== -ShapeFrameCollisionObjectPtr createShapeFrameCollisionObject( +ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, - const ShapePtr& shape, - const BodyNodePtr& bodyNode) + const ShapeNodePtr& shapeNode) { - return collisionDetector->createCollisionObject( - shape, bodyNode); + return collisionDetector->createCollisionObject( + shapeNode->getShape(), shapeNode); } //============================================================================== @@ -181,13 +187,12 @@ std::vector createShapeFrameCollisionObjects( for (auto i = 0u; i < numBodyNodes; ++i) { auto bodyNode = skel->getBodyNode(i); - auto numColShapes = bodyNode->getNumCollisionShapes(); + auto collisionShapeNodes = bodyNode->getShapeNodesWith(); - for (auto j = 0u; j < numColShapes; ++j) + for (auto& shapeNode : collisionShapeNodes) { - auto shape = bodyNode->getCollisionShape(j); - auto collObj = - createShapeFrameCollisionObject(collisionDetector, shape, bodyNode); + auto collObj + = createShapeNodeCollisionObject(collisionDetector, shapeNode); objects.push_back( std::static_pointer_cast(collObj)); diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index 5953cfd0d1098..b0a5828b8f6fd 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -57,7 +57,7 @@ struct BodyNodeCollisionFilter : collision::CollisionFilter const BodyNode* bodyNode2) const; }; -class ShapeFrameCollisionObject : public collision::CollisionObject +class ShapeNodeCollisionObject : public collision::CollisionObject { public: @@ -69,31 +69,37 @@ class ShapeFrameCollisionObject : public collision::CollisionObject // Documentation inherited bool isEqual(const CollisionObject* other) const override; - /// Return BodyNode pointer associated with this ShapeFrameCollisionObject - dynamics::BodyNodePtr getBodyNode() const; + /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject + dynamics::ShapeNode* getShapeNode(); + + /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject + const dynamics::ShapeNode* getShapeNode() const; + + /// Return BodyNode pointer associated with this ShapeNodeCollisionObject + dynamics::BodyNode* getBodyNode(); + + /// Return BodyNode pointer associated with this ShapeNodeCollisionObject + const dynamics::BodyNode* getBodyNode() const; protected: - ShapeFrameCollisionObject( + ShapeNodeCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, const ShapePtr& shape, - const BodyNodePtr& bodyNode); - // TODO(JS): this should be replaced by ShapeNode + const ShapeNodePtr& shapeNode); - dynamics::BodyNodePtr mBodyNode; - // TODO(JS): this should be changed to ShapeNode + dynamics::ShapeNodePtr mShapeNode; }; -using ShapeFrameCollisionObjectPtr = std::shared_ptr; +using ShapeNodeCollisionObjectPtr = std::shared_ptr; -/// Create a ShapeFrameCollisionObject given ShapeNode -ShapeFrameCollisionObjectPtr createShapeFrameCollisionObject( +/// Create a ShapeNodeCollisionObjectPtr given ShapeNode +ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( const collision::CollisionDetectorPtr& collisionDetector, - const ShapePtr& shape, - const BodyNodePtr& bodyNode); + const ShapeNodePtr& shapeNode); -/// Create a ShapeFrameCollisionObjects given Skeleton +/// Create a ShapeNodeCollisionObjectPtr given Skeleton std::vector createShapeFrameCollisionObjects( const collision::CollisionDetectorPtr& collisionDetector, const dynamics::SkeletonPtr& skel); diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index 652b5bfbc4704..8244b73ee0a19 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -472,8 +472,8 @@ int main(int argc, char* argv[]) world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); #ifdef HAVE_BULLET_COLLISION - world->getConstraintSolver()->setCollisionEngine( - dart::collision::BulletEngine::create()); + world->getConstraintSolver()->setCollisionDetector( + dart::collision::BulletCollisionDetector::create()); #endif world->addSkeleton(floor); diff --git a/tutorials/tutorialBiped.cpp b/tutorials/tutorialBiped.cpp index 01f56ffbd7bcb..5c5e129e373d3 100644 --- a/tutorials/tutorialBiped.cpp +++ b/tutorials/tutorialBiped.cpp @@ -310,8 +310,8 @@ int main(int argc, char* argv[]) world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); #ifdef HAVE_BULLET_COLLISION - world->getConstraintSolver()->setCollisionEngine( - dart::collision::BulletEngine::create()); + world->getConstraintSolver()->setCollisionDetector( + dart::collision::BulletCollisionDetector::create()); #endif world->addSkeleton(floor); diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 98a379d38067a..0390f3f11e62a 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -597,22 +597,22 @@ void testBodyNodes() auto boxBody1 = boxSkel1->getBodyNode(0u); auto boxBody2 = boxSkel2->getBodyNode(0u); - auto boxShape1 = boxBody1->getCollisionShape(0u); - auto boxShape2 = boxBody2->getCollisionShape(0u); + auto boxShapeNode1 = boxBody1->getShapeNodesWith()[0]; + auto boxShapeNode2 = boxBody2->getShapeNodesWith()[0]; collision::Option option; collision::Result result; - auto obj1 = cd->template createCollisionObject( - boxShape1, boxBody1); - auto obj2 = cd->template createCollisionObject( - boxShape2, boxBody2); + auto obj1 = cd->template createCollisionObject( + boxShapeNode1->getShape(), boxShapeNode1); + auto obj2 = cd->template createCollisionObject( + boxShapeNode2->getShape(), boxShapeNode2); EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); EXPECT_TRUE(cd->detect(obj1.get(), obj2.get(), option, result)); - auto obj3 = cd->template createCollisionObject( - boxShape2, boxBody2); + auto obj3 = cd->template createCollisionObject( + boxShapeNode2->getShape(), boxShapeNode2); EXPECT_FALSE(obj1->isEqual(obj2.get())); EXPECT_FALSE(obj2->isEqual(obj1.get())); From 87e17d99dfc49225ca9b84d0084aada8706d7536 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 11 Mar 2016 16:47:49 -0500 Subject: [PATCH 18/67] Use internal ResourceRetriever when nullptr is passed to parsers --- dart/utils/SkelParser.cpp | 1 + dart/utils/VskParser.cpp | 17 ++++++++++++++++- dart/utils/VskParser.h | 2 +- dart/utils/sdf/SdfParser.cpp | 32 ++++++++++++++++++++++++++------ 4 files changed, 44 insertions(+), 8 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 9217f5f74731f..2ea80be0437b1 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -2391,6 +2391,7 @@ JointPropPtr readFreeJoint( properties); } +//============================================================================== common::ResourceRetrieverPtr getRetriever( const common::ResourceRetrieverPtr& _retriever) { diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index cc52a77ae008d..6212dad9acd18 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -155,6 +155,9 @@ void tokenize(const std::string& str, std::vector& tokens, const std::string& delimiters = " "); +common::ResourceRetrieverPtr getRetriever( + const common::ResourceRetrieverPtr& retriever); + } // anonymous namespace //============================================================================== @@ -183,8 +186,10 @@ VskParser::Options::Options( //============================================================================== dynamics::SkeletonPtr VskParser::readSkeleton(const common::Uri& fileUri, - const Options options) + Options options) { + options.retrieverOrNullptr = getRetriever(options.retrieverOrNullptr); + // Load VSK file and create document tinyxml2::XMLDocument vskDocument; try @@ -1016,6 +1021,16 @@ void tokenize(const std::string& str, } } +//============================================================================== +common::ResourceRetrieverPtr getRetriever( + const common::ResourceRetrieverPtr& retriever) +{ + if(retriever) + return retriever; + else + return std::make_shared(); +} + } // anonymous namespace } // namespace utils diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h index d3e8852670e26..f13510fc7d51c 100644 --- a/dart/utils/VskParser.h +++ b/dart/utils/VskParser.h @@ -100,7 +100,7 @@ namespace VskParser /// Read Skeleton from VSK file dynamics::SkeletonPtr readSkeleton(const common::Uri& fileUri, - const Options options = Options()); + Options options = Options()); } // namespace VskParser diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 6380cdb69db3f..8ad4e2dbdcbe1 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -246,6 +246,9 @@ dynamics::FreeJoint::Properties readFreeJoint( const Eigen::Isometry3d& parentModelFrame, const std::string& name); +common::ResourceRetrieverPtr getRetriever( + const common::ResourceRetrieverPtr& retriever); + } // anonymous namespace @@ -253,15 +256,19 @@ dynamics::FreeJoint::Properties readFreeJoint( //============================================================================== simulation::WorldPtr readSdfFile( - const common::Uri& fileUri, const common::ResourceRetrieverPtr& retriever) + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& nullOrRetriever) { - return readWorld(fileUri, retriever); + return readWorld(fileUri, nullOrRetriever); } //============================================================================== -simulation::WorldPtr readWorld(const common::Uri& fileUri, - const common::ResourceRetrieverPtr& retriever) +simulation::WorldPtr readWorld( + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& nullOrRetriever) { + const auto retriever = getRetriever(nullOrRetriever); + //-------------------------------------------------------------------------- // Load xml and create Document tinyxml2::XMLDocument _dartFile; @@ -312,8 +319,11 @@ simulation::WorldPtr readWorld(const common::Uri& fileUri, //============================================================================== dynamics::SkeletonPtr readSkeleton( - const common::Uri& fileUri, const common::ResourceRetrieverPtr& retriever) + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& nullOrRetriever) { + const auto retriever = getRetriever(nullOrRetriever); + //-------------------------------------------------------------------------- // Load xml and create Document tinyxml2::XMLDocument _dartFile; @@ -1472,7 +1482,17 @@ dynamics::FreeJoint::Properties readFreeJoint( return dynamics::FreeJoint::Properties(); } -} // anonymouse +//============================================================================== +common::ResourceRetrieverPtr getRetriever( + const common::ResourceRetrieverPtr& retriever) +{ + if(retriever) + return retriever; + else + return std::make_shared(); +} + +} // anonymous namespace } // namespace SdfParser From ba69ba7bf2f2ad140e75446f961b9cae20f5718a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 15 Mar 2016 16:33:26 -0400 Subject: [PATCH 19/67] Fix bugs in post processing of each collision detectors --- .../bullet/BulletCollisionDetector.cpp | 2 +- dart/collision/dart/DARTCollisionDetector.cpp | 29 ++-- dart/collision/fcl/FCLCollisionDetector.cpp | 75 +++++------ .../fcl/FCLMeshCollisionDetector.cpp | 124 +++++++----------- 4 files changed, 91 insertions(+), 139 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index f74b1de2b6c58..c0baa1e90f969 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -240,7 +240,7 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, Contact contact; contact.point = convertVector3(bulletManifoldPoint.getPositionWorldOnA()); - contact.normal = convertVector3(bulletManifoldPoint.m_normalWorldOnB); + contact.normal = convertVector3(-bulletManifoldPoint.m_normalWorldOnB); contact.penetrationDepth = -bulletManifoldPoint.m_distance1; contact.collisionObject1 = userData1->collisionObject; contact.collisionObject2 = userData2->collisionObject; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 73a34385f6564..7351b8d9c6b98 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -256,30 +256,29 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, if (pairResult.contacts.empty()) return; - // Remove all the repeated points + // Don't add repeated points const auto tol = 3.0e-12; - auto i = pairResult.contacts.begin(); - while (i != pairResult.contacts.end() - 1) + + for (auto pairContact : pairResult.contacts) { - for (auto j = i + 1; j != pairResult.contacts.end(); ++j) + auto foundClose = false; + + for (auto totalContact : totalResult.contacts) { - if (isClose(i->point, j->point, tol)) + if (isClose(pairContact.point, totalContact.point, tol)) + { + foundClose = true; break; + } } - totalResult.contacts.push_back(*i); + if (foundClose) + continue; + + totalResult.contacts.push_back(pairContact); totalResult.contacts.back().collisionObject1 = o1; totalResult.contacts.back().collisionObject2 = o2; - - ++i; } - -// for (auto& contact : pairResult.contacts) -// { -// totalResult.contacts.push_back(contact); -// totalResult.contacts.back().collisionObject1 = o1; -// totalResult.contacts.back().collisionObject2 = o2; -// } } } // anonymous namespace diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index c9510daffb3ef..b252c1bf5b8f3 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -554,20 +554,19 @@ boost::shared_ptr createFCLCollisionGeometry( case Shape::BOX: { assert(dynamic_cast(shape.get())); - const BoxShape* box = static_cast(shape.get()); + + auto box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) + fclCollGeom.reset(createCube(size[0], size[1], size[2])); -#else - fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); -#endif + break; } case Shape::ELLIPSOID: { assert(dynamic_cast(shape.get())); - EllipsoidShape* ellipsoid = static_cast(shape.get()); + auto ellipsoid = static_cast(shape.get()); const Eigen::Vector3d& size = ellipsoid->getSize(); if (ellipsoid->isSphere()) @@ -576,15 +575,8 @@ boost::shared_ptr createFCLCollisionGeometry( } else { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset( - createEllipsoid(ellipsoid->getSize()[0], - ellipsoid->getSize()[1], - ellipsoid->getSize()[2])); -#else - fclCollGeom.reset( - new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); -#endif + createEllipsoid(size[0], size[1], size[2])); } break; @@ -592,60 +584,55 @@ boost::shared_ptr createFCLCollisionGeometry( case Shape::CYLINDER: { assert(dynamic_cast(shape.get())); - const CylinderShape* cylinder - = static_cast(shape.get()); - const double radius = cylinder->getRadius(); - const double height = cylinder->getHeight(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) + + const auto cylinder = static_cast(shape.get()); + const auto radius = cylinder->getRadius(); + const auto height = cylinder->getHeight(); + fclCollGeom.reset(createCylinder( radius, radius, height, 16, 16)); -#else - fclCollGeom.reset(new fcl::Cylinder(radius, height)); -#endif - // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 - // returns single contact point for cylinder yet. + break; } case Shape::PLANE: { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); -#else - assert(dynamic_cast(shape.get())); - dynamics::PlaneShape* plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - fclCollGeom.reset( - new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); -#endif + + dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " + << "FCLCollisionDetector. We create a thin box mesh insted, where " + << "the size is [1000 0 1000].\n"; + break; } case Shape::MESH: { assert(dynamic_cast(shape.get())); - MeshShape* shapeMesh = static_cast(shape.get()); + + auto shapeMesh = static_cast(shape.get()); + const Eigen::Vector3d& scale = shapeMesh->getScale(); + auto aiScene = shapeMesh->getMesh(); + fclCollGeom.reset( - createMesh(shapeMesh->getScale()[0], - shapeMesh->getScale()[1], - shapeMesh->getScale()[2], - shapeMesh->getMesh())); + createMesh(scale[0], scale[1], scale[2], aiScene)); break; } case Shape::SOFT_MESH: { assert(dynamic_cast(shape.get())); - SoftMeshShape* softMeshShape = static_cast(shape.get()); - fclCollGeom.reset( - createSoftMesh(softMeshShape->getAssimpMesh())); + + auto softMeshShape = static_cast(shape.get()); + auto aiMesh = softMeshShape->getAssimpMesh(); + + fclCollGeom.reset(createSoftMesh(aiMesh)); break; } default: { - dterr << "[FCLCollisionObjectData::updateShape] " - << "Attempting to create unsupported shape type '" - << shape->getShapeType() << "'.\n"; + dterr << "[FCLCollisionDetector] Attempting to create unsupported shape " + << "type '" << shape->getShapeType() << "'.\n"; + return nullptr; } } diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp index 97f3c76aedeef..21e016bf90b48 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl/FCLMeshCollisionDetector.cpp @@ -119,14 +119,12 @@ struct FCLCollisionCallbackData } }; -int evalContactPosition( - const fcl::Contact& fclContact, +int evalContactPosition(const fcl::Contact& fclContact, const fcl::BVHModel* mesh1, const fcl::BVHModel* mesh2, const fcl::Transform3f& transform1, const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition1, - Eigen::Vector3d* contactPosition2); + Eigen::Vector3d* contactPosition); bool isColinear(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, @@ -918,13 +916,14 @@ void postProcess(const fcl::CollisionResult& fclResult, int numContacts = 0; std::vector unfiltered; - unfiltered.reserve(fclResult.numContacts()); + unfiltered.reserve(initNumContacts * 2); - for (auto k = 0u; k < fclResult.numContacts(); ++k) + for (auto k = 0u; k < initNumContacts; ++k) { // for each pair of intersecting triangles, we create two contact points - Contact pair1, pair2; - const fcl::Contact& c = fclResult.getContact(k); + Contact pair1; + Contact pair2; + const auto& c = fclResult.getContact(k); auto userData1 = static_cast(o1->getUserData()); @@ -942,7 +941,7 @@ void postProcess(const fcl::CollisionResult& fclResult, static_cast*>(c.o2), FCLTypes::convertTransform(collisionObject1->getTransform()), FCLTypes::convertTransform(collisionObject2->getTransform()), - &pair1.point, &pair2.point); + &pair1.point); if (contactResult == COPLANAR_CONTACT) { @@ -962,12 +961,10 @@ void postProcess(const fcl::CollisionResult& fclResult, numContacts++; } - pair1.normal = FCLTypes::convertVector3(-fclResult.getContact(k).normal); - pair2.normal = pair1.normal; + pair1.normal = FCLTypes::convertVector3(-c.normal); pair1.penetrationDepth = c.penetration_depth; pair1.triID1 = c.b1; pair1.triID2 = c.b2; - pair1.collisionObject1 = collisionObject1; pair1.collisionObject2 = collisionObject2; @@ -976,11 +973,7 @@ void postProcess(const fcl::CollisionResult& fclResult, unfiltered.push_back(pair2); } - - - - - const double tol = 1e-12; + const auto tol = 1e-12; auto unfilteredSize = unfiltered.size(); @@ -997,7 +990,7 @@ void postProcess(const fcl::CollisionResult& fclResult, const auto diff = contact1.point - contact2.point; - if (diff.norm() < 3.0 * tol) + if (diff.dot(diff) < 3.0 * tol) { markForDeletion[i] = true; break; @@ -1006,71 +999,44 @@ void postProcess(const fcl::CollisionResult& fclResult, } // remove all the co-linear contact points -// for (auto i = 0u; i < initNumContacts; ++i) -// { -// if (markForDeletion[i]) -// continue; + for (auto i = 0u; i < unfilteredSize; ++i) + { + if (markForDeletion[i]) + continue; -// const auto& contact1 = unfiltered[i]; + const auto& contact1 = unfiltered[i]; -// for (auto j = i + 1u; j < initNumContacts; ++j) -// { -// if (markForDeletion[j]) -// continue; + for (auto j = 0u; j < unfilteredSize; ++j) + { + if (i == j || markForDeletion[j]) + continue; -// const auto& contact2 = unfiltered[j]; + if (markForDeletion[i]) + break; -// for (auto k = j + 1u; k < initNumContacts; ++k) -// { -// if (markForDeletion[k]) -// continue; + const auto& contact2 = unfiltered[j]; -// const auto& contact3 = unfiltered[k]; + for (auto k = j + 1u; k < unfilteredSize; ++k) + { + if (i == k) + continue; -// if (isColinear(contact1.point, contact2.point, contact3.point, tol)) -// { -// markForDeletion[i] = true; -// break; -// } -// } -// } -// } + const auto& contact3 = unfiltered[k]; - for (size_t i = 0; i < initNumContacts; ++i) - { -// if (!markForDeletion[i]) - { - const auto& contact = unfiltered[i]; - result.contacts.push_back(contact); + if (isColinear(contact1.point, contact2.point, contact3.point, tol)) + { + markForDeletion[i] = true; + break; + } + } } } - - - -// for (auto i = 0u; i < numContacts; ++i) -// { -// const auto fclContact = fclResult.getContact(i); -// result.contacts.push_back(convertContact(fclContact, o1, o2)); -// } - - - -// std::cout << "=================" << std::endl; -// std::cout << "# of contacts:" << result.contacts.size() << std::endl; -// std::cout << "=================" << std::endl; -// for (auto i = 0u; i < result.contacts.size(); ++i) -// { -// auto contact = result.contacts[i]; - -// std::cout << "Contact (" << i << ")" << std::endl; -// std::cout << "Point : " << contact.point.transpose() << std::endl; -// std::cout << "Normal: " << contact.normal.transpose() << std::endl; -// std::cout << "Depth : " << contact.penetrationDepth << std::endl; -// std::cout << std::endl; -// } -// std::cout << "=================" << std::endl; - + for (auto i = 0u; i < unfilteredSize; ++i) + { + if (!markForDeletion[i]) + result.contacts.push_back(unfiltered[i]); + } } //============================================================================== @@ -1080,8 +1046,7 @@ int evalContactPosition( const fcl::BVHModel* mesh2, const fcl::Transform3f& transform1, const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition1, - Eigen::Vector3d* contactPosition2) + Eigen::Vector3d* contactPosition) { int id1 = fclContact.b1; int id2 = fclContact.b2; @@ -1122,8 +1087,7 @@ int evalContactPosition( contact2 = contact1; } - *contactPosition1 = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); - *contactPosition2 = Eigen::Vector3d(contact2[0], contact2[1], contact2[2]); + *contactPosition = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); return testRes; } @@ -1173,9 +1137,11 @@ bool isColinear(const Eigen::Vector3d& pos1, const auto vb = pos1 - pos3; const auto v = va.cross(vb); - return v.norm() < tol; -} + const auto cond1 = v.dot(v) < tol; + const auto cond2 = va.dot(vb) < 0.0; + return cond1 && cond2; +} //============================================================================== void convertOption(const Option& fclOption, fcl::CollisionRequest& request) From d85b74bc574fc2bf65726a00c1c1f384fb566225 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 23 Mar 2016 17:55:39 -0400 Subject: [PATCH 20/67] Hiding CollisionObject from public API --- dart/collision/CollisionDetector.cpp | 204 +++++++++----- dart/collision/CollisionDetector.h | 100 ++++--- dart/collision/CollisionGroup.cpp | 229 ++++++--------- dart/collision/CollisionGroup.h | 101 +++---- dart/collision/CollisionGroupData.cpp | 80 ------ dart/collision/CollisionGroupData.h | 99 ------- dart/collision/CollisionObject.cpp | 168 ++++++----- dart/collision/CollisionObject.h | 115 ++++---- dart/collision/CollisionObjectData.cpp | 77 ----- dart/collision/CollisionObjectData.h | 81 ------ dart/collision/SmartPointer.h | 4 - .../bullet/BulletCollisionDetector.cpp | 253 ++++++++--------- .../bullet/BulletCollisionDetector.h | 71 ++--- ...GroupData.cpp => BulletCollisionGroup.cpp} | 118 +++++--- ...sionGroupData.h => BulletCollisionGroup.h} | 51 ++-- ...jectData.cpp => BulletCollisionObject.cpp} | 34 +-- ...onObjectData.h => BulletCollisionObject.h} | 25 +- dart/collision/dart/DARTCollisionDetector.cpp | 125 ++++----- dart/collision/dart/DARTCollisionDetector.h | 39 +-- ...onGroupData.cpp => DARTCollisionGroup.cpp} | 60 ++-- ...lisionGroupData.h => DARTCollisionGroup.h} | 46 +-- ...ObjectData.cpp => DARTCollisionObject.cpp} | 18 +- ...sionObjectData.h => DARTCollisionObject.h} | 23 +- dart/collision/detail/CollisionDetector.h | 20 -- dart/collision/fcl/FCLCollisionDetector.cpp | 264 +++++++++--------- dart/collision/fcl/FCLCollisionDetector.h | 55 ++-- ...ionGroupData.cpp => FCLCollisionGroup.cpp} | 108 +++---- ...llisionGroupData.h => FCLCollisionGroup.h} | 59 ++-- ...nObjectData.cpp => FCLCollisionObject.cpp} | 62 ++-- ...isionObjectData.h => FCLCollisionObject.h} | 34 +-- .../fcl/FCLMeshCollisionDetector.cpp | 253 ++++++++--------- dart/collision/fcl/FCLMeshCollisionDetector.h | 51 ++-- ...roupData.cpp => FCLMeshCollisionGroup.cpp} | 99 +++---- ...ionGroupData.h => FCLMeshCollisionGroup.h} | 51 ++-- ...ectData.cpp => FCLMeshCollisionObject.cpp} | 60 ++-- ...nObjectData.h => FCLMeshCollisionObject.h} | 30 +- dart/constraint/ConstraintSolver.cpp | 36 ++- dart/constraint/ContactConstraint.cpp | 16 +- dart/constraint/SoftContactConstraint.cpp | 31 +- dart/dynamics/CollisionDetector.cpp | 258 ++++++++--------- dart/dynamics/CollisionDetector.h | 72 ++--- dart/dynamics/Frame.cpp | 12 + dart/dynamics/Frame.h | 8 + dart/dynamics/ShapeFrame.cpp | 33 +++ dart/dynamics/ShapeFrame.h | 20 ++ dart/dynamics/ShapeNode.cpp | 12 + dart/dynamics/ShapeNode.h | 6 + dart/dynamics/SimpleFrame.cpp | 31 ++ dart/dynamics/SimpleFrame.h | 18 ++ osgDart/WorldNode.cpp | 7 +- tutorials/tutorialCollisions-Finished.cpp | 2 +- tutorials/tutorialDominoes-Finished.cpp | 2 +- unittests/testCollision.cpp | 122 ++++---- 53 files changed, 1874 insertions(+), 2079 deletions(-) delete mode 100644 dart/collision/CollisionGroupData.cpp delete mode 100644 dart/collision/CollisionGroupData.h delete mode 100644 dart/collision/CollisionObjectData.cpp delete mode 100644 dart/collision/CollisionObjectData.h rename dart/collision/bullet/{BulletCollisionGroupData.cpp => BulletCollisionGroup.cpp} (54%) rename dart/collision/bullet/{BulletCollisionGroupData.h => BulletCollisionGroup.h} (71%) rename dart/collision/bullet/{BulletCollisionObjectData.cpp => BulletCollisionObject.cpp} (81%) rename dart/collision/bullet/{BulletCollisionObjectData.h => BulletCollisionObject.h} (82%) rename dart/collision/dart/{DARTCollisionGroupData.cpp => DARTCollisionGroup.cpp} (65%) rename dart/collision/dart/{DARTCollisionGroupData.h => DARTCollisionGroup.h} (66%) rename dart/collision/dart/{DARTCollisionObjectData.cpp => DARTCollisionObject.cpp} (83%) rename dart/collision/dart/{DARTCollisionObjectData.h => DARTCollisionObject.h} (79%) rename dart/collision/fcl/{FCLMeshCollisionGroupData.cpp => FCLCollisionGroup.cpp} (53%) rename dart/collision/fcl/{FCLMeshCollisionGroupData.h => FCLCollisionGroup.h} (64%) rename dart/collision/fcl/{FCLCollisionObjectData.cpp => FCLCollisionObject.cpp} (80%) rename dart/collision/fcl/{FCLCollisionObjectData.h => FCLCollisionObject.h} (76%) rename dart/collision/fcl/{FCLCollisionGroupData.cpp => FCLMeshCollisionGroup.cpp} (56%) rename dart/collision/fcl/{FCLCollisionGroupData.h => FCLMeshCollisionGroup.h} (69%) rename dart/collision/fcl/{FCLMeshCollisionObjectData.cpp => FCLMeshCollisionObject.cpp} (80%) rename dart/collision/fcl/{FCLMeshCollisionObjectData.h => FCLMeshCollisionObject.h} (77%) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 7023c2be3c53e..edaefdc01458d 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -53,74 +53,96 @@ namespace collision { //============================================================================== CollisionDetector::~CollisionDetector() { -// assert(mCollisionObjects.empty()); + assert(mCollisionObjectMap.empty()); } //============================================================================== std::shared_ptr CollisionDetector::createCollisionGroup( - const std::vector& objects) + dynamics::Skeleton* skel) { - return std::make_shared(shared_from_this(), objects); -} - -//============================================================================== -bool CollisionDetector::detect( - CollisionObject* object1, CollisionObject* object2, - const Option& option, Result& result) -{ - assert(object1->getCollisionDetector() == object2->getCollisionDetector()); - - object1->updateEngineData(); - object2->updateEngineData(); - - return detect(object1->getEngineData(), object2->getEngineData(), - option, result); -} - -//============================================================================== -bool CollisionDetector::detect( - CollisionObject* object, CollisionGroup* group, - const Option& option, Result& result) -{ - assert(object->getCollisionDetector() == group->getCollisionDetector()); + assert(skel); - object->updateEngineData(); - group->updateEngineData(); + auto group = createCollisionGroup(); - return detect(object->getEngineData(), group->getEngineData(), - option, result); -} - -//============================================================================== -bool CollisionDetector::detect( - CollisionGroup* group, CollisionObject* object, - const Option& option, Result& result) -{ - return detect(object, group, option, result); -} + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + { + auto bodyNode = skel->getBodyNode(i); + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); -//============================================================================== -bool CollisionDetector::detect( - CollisionGroup* group, const Option& option, Result& result) -{ - group->updateEngineData(); + for (auto& shapeNode : collisionShapeNodes) + group->addShapeFrame(shapeNode); + } - return detect(group->getEngineData(), option, result); + return group; } -//============================================================================== -bool CollisionDetector::detect( - CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) -{ - assert(group1->getCollisionDetector() == group2->getCollisionDetector()); - - group1->updateEngineData(); - group2->updateEngineData(); - - return detect(group1->getEngineData(), group2->getEngineData(), - option, result); -} +////============================================================================== +//std::shared_ptr CollisionDetector::createCollisionGroup( +// const std::vector& objects) +//{ +// return std::make_shared(shared_from_this(), objects); +//} + +////============================================================================== +//bool CollisionDetector::detect( +// CollisionObject* object1, CollisionObject* object2, +// const Option& option, Result& result) +//{ +// assert(object1->getCollisionDetector() == object2->getCollisionDetector()); + +// object1->updateEngineData(); +// object2->updateEngineData(); + +// return detect(object1->getEngineData(), object2->getEngineData(), +// option, result); +//} + +////============================================================================== +//bool CollisionDetector::detect( +// CollisionObject* object, CollisionGroup* group, +// const Option& option, Result& result) +//{ +// assert(object->getCollisionDetector() == group->getCollisionDetector()); + +// object->updateEngineData(); +// group->updateEngineData(); + +// return detect(object->getEngineData(), group->getEngineData(), +// option, result); +//} + +////============================================================================== +//bool CollisionDetector::detect( +// CollisionGroup* group, CollisionObject* object, +// const Option& option, Result& result) +//{ +// return detect(object, group, option, result); +//} + +////============================================================================== +//bool CollisionDetector::detect( +// CollisionGroup* group, const Option& option, Result& result) +//{ +// group->update(); + +// return detect(group, option, result); +//} + +////============================================================================== +//bool CollisionDetector::detect( +// CollisionGroup* group1, CollisionGroup* group2, +// const Option& option, Result& result) +//{ +// assert(group1->getCollisionDetector() == group2->getCollisionDetector()); + +// group1->updateEngineData(); +// group2->updateEngineData(); + +// return detect(group1->getEngineData(), group2->getEngineData(), +// option, result); +//} //============================================================================== struct is_collision_object_equal @@ -141,33 +163,69 @@ struct is_collision_object_equal }; //============================================================================== -void CollisionDetector::reclaimCollisionObject(CollisionObject* collisionObject) +CollisionObject* CollisionDetector::claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { - assert(hasCollisionObject(collisionObject)); + auto search = mCollisionObjectMap.find(shapeFrame); - mCollisionObjects.erase( - std::remove_if(mCollisionObjects.begin(), mCollisionObjects.end(), - is_collision_object_equal(collisionObject)), - mCollisionObjects.end()); + // Found existing collision object + if (mCollisionObjectMap.end() != search) + { + auto& collObjAndCount = search->second; + + auto& collObj = collObjAndCount.first; + auto& count = collObjAndCount.second; + assert(count != 0u); + + count++; + + return collObj.get(); + } - reclaimCollisionObjectData(collisionObject->getEngineData()); + auto newCollisionObject = createCollisionObject(shapeFrame); + + mCollisionObjectMap[shapeFrame] + = std::make_pair(std::move(newCollisionObject), 1u); + + return mCollisionObjectMap[shapeFrame].first.get(); } //============================================================================== -bool CollisionDetector::hasCollisionObject( - const CollisionObject* collisionObject) const +void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) { - return std::find_if(mCollisionObjects.begin(), mCollisionObjects.end(), - is_collision_object_equal(collisionObject)) - != mCollisionObjects.end(); + auto shapeFrame = collObj->getShapeFrame(); + auto search = mCollisionObjectMap.find(shapeFrame); + + if (mCollisionObjectMap.end() == search) + return; + + auto& collObjAndCount = search->second; + auto& count = collObjAndCount.second; + assert(count != 0u); + + count--; + + if (0u == count) + { + auto& collisionObject = collObjAndCount.first; + notifyDestroyingCollisionObject(collisionObject.get()); + + mCollisionObjectMap.erase(search); + } } //============================================================================== -bool CollisionDetector::detect( - CollisionGroupData* group, CollisionObjectData* object, - const Option& option, Result& result) +bool CollisionDetector::hasCollisionObject(const CollisionObject* collObj) const { - return detect(object, group, option, result); + if (!collObj) + return false; + + return true; +// return std::find_if( +// mCollisionObjectMap.begin(), mCollisionObjectMap.end(), +// [&](const CollisionObjectPair& pair) +// { return pair.second.first.get() == collObj; }) +// != mCollisionObjectMap.end(); } } // namespace collision diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 1c9ed2946c313..53a1e276fcf09 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -67,36 +67,40 @@ class CollisionDetector : public std::enable_shared_from_this /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; - /// Create a collision object - template - std::shared_ptr createCollisionObject( - const dynamics::ShapePtr& shape, - const Args&... args); - /// Create a collision group + virtual std::shared_ptr createCollisionGroup() = 0; + + /// Create a collision group from ShapeFrame + virtual std::shared_ptr createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) = 0; + + /// Create a collision group from ShapeFrames + virtual std::shared_ptr createCollisionGroup( + const std::vector& shapeFrames) = 0; + + /// Create a collision group from Skeleton std::shared_ptr createCollisionGroup( - const std::vector& objects - = std::vector()); + dynamics::Skeleton* skeleton); - /// Perform collision detection for object1-object2. - bool detect(CollisionObject* object1, CollisionObject* object2, - const Option& option, Result& result); +// /// Perform collision detection for object1-object2. +// bool detect(CollisionObject* object1, CollisionObject* object2, +// const Option& option, Result& result); - /// Perform collision detection for object-group. - bool detect(CollisionObject* object, CollisionGroup* group, - const Option& option, Result& result); +// /// Perform collision detection for object-group. +// bool detect(CollisionObject* object, CollisionGroup* group, +// const Option& option, Result& result); - /// Identical with detect(object, group, option, result) - bool detect(CollisionGroup* group, CollisionObject* object, - const Option& option, Result& result); +// /// Identical with detect(object, group, option, result) +// bool detect(CollisionGroup* group, CollisionObject* object, +// const Option& option, Result& result); /// Perform collision detection for group. - bool detect(CollisionGroup* group, - const Option& option, Result& result); +// bool detect(CollisionGroup* group, +// const Option& option, Result& result); - /// Perform collision detection for group1-group2. - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result); +// /// Perform collision detection for group1-group2. +// bool detect(CollisionGroup* group1, CollisionGroup* group2, +// const Option& option, Result& result); protected: @@ -105,51 +109,43 @@ class CollisionDetector : public std::enable_shared_from_this /// Constructor CollisionDetector() = default; - /// Reclaim collisionObject from this CollisionDetector. - void reclaimCollisionObject(CollisionObject* collisionObject); + /// Return CollisionObject associated with shapeFrame. New CollisionObject + /// will be created if it hasn't created yet for shapeFrame. + CollisionObject* claimCollisionObject(const dynamics::ShapeFrame* shapeFrame); + // TODO(JS): Maybe WeakShapeFramePtr - /// Create collision detection engine specific data for CollisionObject - virtual std::unique_ptr createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) = 0; + /// Create CollisionObject + virtual std::unique_ptr createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) = 0; - /// Reclaim collision detection engine specific data for CollisionObject - virtual void reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) = 0; + /// + virtual void notifyDestroyingCollisionObject(CollisionObject* collObj) = 0; - /// Create collision detection engine specific data for CollisionGroup - virtual std::unique_ptr createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) = 0; + /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject + /// will be destroyed if no CollisionGroup holds it. + void reclaimCollisionObject(const CollisionObject* shapeFrame); /// Return true if collisionObject is created from this CollisionDetector bool hasCollisionObject(const CollisionObject* collisionObject) const; - /// Perform collision detection for object1-object2. - virtual bool detect(CollisionObjectData* object1, - CollisionObjectData* object2, - const Option& option, Result& result) = 0; - - /// Perform collision detection for object-group. - virtual bool detect(CollisionObjectData* object, CollisionGroupData* group, - const Option& option, Result& result) = 0; - - /// Identical with detect(object, group, option, result) - bool detect(CollisionGroupData* group, CollisionObjectData* object, - const Option& option, Result& result); - /// Perform collision detection for group. - virtual bool detect(CollisionGroupData* group, + virtual bool detect(CollisionGroup* group, const Option& option, Result& result) = 0; /// Perform collision detection for group1-group2. - virtual bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) = 0; protected: - /// CollisionObject array created from this CollisionDetector - std::vector mCollisionObjects; + using CollisionObjectMapValue + = std::pair, size_t>; + using CollisionObjectMap + = std::map; + using CollisionObjectPair + = std::pair; + + CollisionObjectMap mCollisionObjectMap; }; diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 9107cd6ca052b..8e2b703bf54b8 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -36,238 +36,191 @@ #include "dart/collision/CollisionGroup.h" +#include + #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionGroupData.h" +#include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionDetector.h" - -#include +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" namespace dart { namespace collision { //============================================================================== -CollisionGroup::CollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const CollisionGroup::CollisionObjectPtrs& collObjects) - : mCollisionDetector(collisionDetector), - mCollisionObjects(collObjects), - mEngineData(mCollisionDetector->createCollisionGroupData( - this, mCollisionObjects).release()) +CollisionGroup::CollisionGroup(const CollisionDetectorPtr& collisionDetector) + : mCollisionDetector(collisionDetector) { assert(mCollisionDetector); } -//============================================================================== -CollisionGroup::CollisionGroup(const CollisionGroup& other) - : mCollisionDetector(other.mCollisionDetector), - mCollisionObjects(other.mCollisionObjects), - mEngineData(other.mEngineData->clone(this, mCollisionObjects)) -{ - // Do nothing -} - -//============================================================================== -CollisionGroup& CollisionGroup::operator=(const CollisionGroup& other) -{ - copy(other); - return *this; -} - //============================================================================== CollisionGroup::~CollisionGroup() { - // Do nothing + assert(mShapeFrames.empty()); + assert(mCollisionObjects.empty()); } //============================================================================== -void CollisionGroup::copy(const CollisionGroup& other) +CollisionDetector* CollisionGroup::getCollisionDetector() const { - mCollisionDetector = other.mCollisionDetector; - mCollisionObjects = other.mCollisionObjects; - mEngineData.reset( - other.mEngineData->clone(this, mCollisionObjects).release()); + return mCollisionDetector.get(); } //============================================================================== -void CollisionGroup::changeDetector( - const CollisionDetectorPtr& collisionDetector) +void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) { - if (mCollisionDetector == collisionDetector) + if (!shapeFrame) return; -// mEngineData.reset( -// engine->createCollisionGroupData(getCollisionObjects()).release()); - // TODO(JS): Implement this! -} - -//============================================================================== -CollisionDetector* CollisionGroup::getCollisionDetector() const -{ - return mCollisionDetector.get(); -} + if (hasShapeFrame(shapeFrame)) + return; -//============================================================================== -struct is_collision_object_equal - : public std::unary_function -{ - explicit is_collision_object_equal(const CollisionObjectPtr& object) - : mObject(object) {} + auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame); - bool operator() (const CollisionObjectPtr& arg) - { - return mObject->isEqual(arg.get()); - } + addCollisionObjectToEngine(collObj); - const CollisionObjectPtr& mObject; -}; + mShapeFrames.push_back(shapeFrame); + mCollisionObjects.push_back(collObj); +} //============================================================================== -bool CollisionGroup::hasCollisionObject( - const CollisionGroup::CollisionObjectPtr& object) const +void CollisionGroup::addShapeFrames( + const std::vector& shapeFrames) { - return std::find_if(mCollisionObjects.begin(), mCollisionObjects.end(), - is_collision_object_equal(object)) - != mCollisionObjects.end(); + for (const auto& shapeFrame : shapeFrames) + addShapeFrame(shapeFrame); } //============================================================================== -void CollisionGroup::addCollisionObject( - const CollisionObjectPtr& object, bool init) +void CollisionGroup::addShapeFrames(const dynamics::Skeleton* skel) { - if (hasCollisionObject(object)) - return; + assert(skel); - object->addGroup(this); + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + { + auto bodyNode = skel->getBodyNode(i); + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); - mCollisionObjects.push_back(object); - mEngineData->addCollisionObject(object, init); + for (auto& shapeNode : collisionShapeNodes) + addShapeFrame(shapeNode); + } } //============================================================================== -void CollisionGroup::addCollisionObjects( - const CollisionGroup::CollisionObjectPtrs& objects, bool init) +void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) { - for (auto object : objects) - addCollisionObject(object, false); + if (!shapeFrame) + return; - if (init) - mEngineData->init(); -} + const auto search + = std::find_if(mShapeFrames.begin(), mShapeFrames.end(), + [&](const dynamics::ShapeFrame* it) + { return it == shapeFrame; }); -//============================================================================== -void CollisionGroup::removeCollisionObject( - const CollisionGroup::CollisionObjectPtr& object, bool init) -{ - if (!object) + if (mShapeFrames.end() == search) return; - auto result - = std::find(mCollisionObjects.begin(), mCollisionObjects.end(), object); + const size_t index = search - mShapeFrames.begin(); - if (mCollisionObjects.end() != result) - { - mCollisionObjects.erase(result); - mEngineData->removeCollisionObject(*result, false); - } + removeCollisionObjectFromEngine(mCollisionObjects[index]); + + mCollisionDetector->reclaimCollisionObject(mCollisionObjects[index]); - if (init) - mEngineData->init(); + mShapeFrames.erase(search); + mCollisionObjects.erase(mCollisionObjects.begin() + index); } //============================================================================== -void CollisionGroup::removeCollisionObjects( - const CollisionGroup::CollisionObjectPtrs& objects, bool init) +void CollisionGroup::removeShapeFrames( + const std::vector& shapeFrames) { - for (auto object : objects) - removeCollisionObject(object, false); - - if (init) - mEngineData->init(); + for (const auto& shapeFrame : shapeFrames) + removeShapeFrame(shapeFrame); } //============================================================================== -void CollisionGroup::removeAllCollisionObjects(bool init) +void CollisionGroup::removeAllShapeFrames() { - // Notify the engine data first that the collision objects are going to be - // removed, then actually remove all the collision objects. - mEngineData->removeAllCollisionObjects(init); + removeAllCollisionObjectsFromEngine(); + + for (const auto& object : mCollisionObjects) + mCollisionDetector->reclaimCollisionObject(object); + mShapeFrames.clear(); mCollisionObjects.clear(); } //============================================================================== -const CollisionGroup::CollisionObjectPtrs& -CollisionGroup::getCollisionObjects() +bool CollisionGroup::hasShapeFrame( + const dynamics::ShapeFrame* shapeFrame) const { - return mCollisionObjects; + return std::find_if( + mShapeFrames.begin(), mShapeFrames.end(), + [&](const dynamics::ShapeFrame* it) { return it == shapeFrame; }) + != mShapeFrames.end(); } //============================================================================== -const CollisionGroup::ConstCollisionObjectPtrs -CollisionGroup::getCollisionObjects() const +void CollisionGroup::unionGroup(const CollisionGroupPtr& other) { - ConstCollisionObjectPtrs vec(mCollisionObjects.size()); + if (!other) + return; - for (auto i = 0u; i < mCollisionObjects.size(); ++i) + if (mCollisionDetector != other->mCollisionDetector) { - vec[i] = std::const_pointer_cast( - mCollisionObjects[i]); + // TODO(JS): warning message + return; } - return vec; + for (const auto& shapeFrame : other->mShapeFrames) + addShapeFrame(shapeFrame); } //============================================================================== -void CollisionGroup::unionGroup(const CollisionGroupPtr& other) +void CollisionGroup::subtractGroup(const CollisionGroupPtr& other) { if (!other) return; - this->addCollisionObjects(other->getCollisionObjects()); -} - -//============================================================================== -void CollisionGroup::subtractGroup(const CollisionGroupPtr& other) -{ - if (!other) + if (mCollisionDetector != other->mCollisionDetector) + { + // TODO(JS): warning message return; + } - this->removeCollisionObjects(other->getCollisionObjects()); + for (const auto& shapeFrame : other->mShapeFrames) + removeShapeFrame(shapeFrame); } //============================================================================== -bool CollisionGroup::detect(const Option& option, Result& result) +void CollisionGroup::update() { - return mCollisionDetector->detect(this, option, result); -} + for (auto& object : mCollisionObjects) + object->updateEngineData(); -//============================================================================== -bool CollisionGroup::detect(CollisionObject* object, - const Option& option, Result& result) -{ - return mCollisionDetector->detect(object, this, option, result); + updateEngineData(); } //============================================================================== -bool CollisionGroup::detect(CollisionGroup* otherGroup, - const Option& option, Result& result) +bool CollisionGroup::detect(const Option& option, Result& result) { - return mCollisionDetector->detect(this, otherGroup, option, result); + return mCollisionDetector->detect(this, option, result); } //============================================================================== -CollisionGroupData* CollisionGroup::getEngineData() +bool CollisionGroup::detect( + CollisionGroup* other, const Option& option, Result& result) { - return mEngineData.get(); + return mCollisionDetector->detect(this, other, option, result); } //============================================================================== -void CollisionGroup::updateEngineData() +const std::vector& CollisionGroup::getCollisionObjects() { - for (auto collObj : mCollisionObjects) - collObj->updateEngineData(); - - mEngineData->update(); + return mCollisionObjects; } } // namespace collision diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 0bd1c347c4414..f9b4c64f10f72 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -38,9 +38,10 @@ #define DART_COLLISION_COLLISIONGROUP_H_ #include +#include #include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionGroupData.h" +#include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionFilter.h" namespace dart { @@ -50,60 +51,36 @@ class CollisionGroup { public: - using CollisionObjectPtr = std::shared_ptr; - using CollisionObjectPtrs = std::vector; - using ConstCollisionObjectPtrs = std::vector; - /// Constructor - CollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const CollisionObjectPtrs& collObjects = CollisionObjectPtrs()); - - /// Copy constructor - CollisionGroup(const CollisionGroup& other); + CollisionGroup(const CollisionDetectorPtr& collisionDetector); /// Destructor virtual ~CollisionGroup(); - /// Assignment operator - CollisionGroup& operator=(const CollisionGroup& other); - - /// Copy another CollisionGroup into this CollisionGroup - void copy(const CollisionGroup& other); - - /// Change engine - void changeDetector(const CollisionDetectorPtr& collisionDetector); - /// Return collision detection engine associated with this CollisionGroup CollisionDetector* getCollisionDetector() const; - /// Return true if this CollisionGroup contains given object - bool hasCollisionObject(const CollisionObjectPtr& object) const; + /// Add a ShapeFrame to this CollisionGroup + void addShapeFrame(const dynamics::ShapeFrame* shapeFrame); - /// Add collision object to this CollisionGroup - void addCollisionObject(const CollisionObjectPtr& object, - bool init = true); + /// Add ShapeFrames to this CollisionGroup + void addShapeFrames( + const std::vector& shapeFrames); - /// Add collision objects to this CollisionGroup - void addCollisionObjects(const CollisionObjectPtrs& objects, - bool init = true); + void addShapeFrames(const dynamics::Skeleton* skeleton); /// Remove collision object from this CollisionGroup - void removeCollisionObject(const CollisionObjectPtr& object, - bool init = true); + void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame); /// Remove collision objects from this CollisionGroup - void removeCollisionObjects(const CollisionObjectPtrs& objects, - bool init = true); + void removeShapeFrames( + const std::vector& shapeFrames); /// Remove all the collision object in this CollisionGroup - void removeAllCollisionObjects(bool init = true); + void removeAllShapeFrames(); - /// Return array of collision objects - const CollisionObjectPtrs& getCollisionObjects(); - - /// Return array of (const) collision objects - const ConstCollisionObjectPtrs getCollisionObjects() const; + /// Return true if this CollisionGroup contains shapeFrame + bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const; /// Merge other CollisionGroup into this CollisionGroup void unionGroup(const CollisionGroupPtr& other); @@ -111,39 +88,53 @@ class CollisionGroup /// Merge other CollisionGroup into this CollisionGroup void subtractGroup(const CollisionGroupPtr& other); + /// Update engine data. This function should be called before the collision + /// detection is performed by the engine in most cases. + void update(); + /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); - /// Perform collision detection with other CollisionObject. - /// - /// Return false if the engine type of the other CollisionObject is different - /// from this CollisionObject engine. - bool detect(CollisionObject* object, const Option& option, Result& result); - /// Perform collision detection with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. bool detect(CollisionGroup* group, const Option& option, Result& result); - /// Return the collision detection engine specific data of this - /// CollisionObject - CollisionGroupData* getEngineData(); + const std::vector& getCollisionObjects(); - /// Update engine data. This function should be called before the collision - /// detection is performed by the engine in most cases. - void updateEngineData(); +protected: + + /// Initialize the collision group data of the collision detection engine such + /// as broadphase algorithm. This function will be called whenever ShapeFrame + /// is either added to or removed from this CollisionGroup. + virtual void initializeEngineData() = 0; + + virtual void addCollisionObjectToEngine(CollisionObject* object) = 0; + + virtual void addCollisionObjectsToEngine( + const std::vector& collObjects) = 0; + + virtual void removeCollisionObjectFromEngine(CollisionObject* object) = 0; + + virtual void removeAllCollisionObjectsFromEngine() = 0; + + /// Update the collision group of the collision detection engine such as + /// broadphase algorithm. This function will be called ahead of every + /// collision checking. + virtual void updateEngineData() = 0; + + using CollisionObjectPtr = std::shared_ptr; + using CollisionObjectPtrs = std::vector; + using ConstCollisionObjectPtrs = std::vector; protected: /// Collision detector CollisionDetectorPtr mCollisionDetector; - /// Collision objects - CollisionObjectPtrs mCollisionObjects; - - /// Engine specific data - std::unique_ptr mEngineData; + std::vector mShapeFrames; + std::vector mCollisionObjects; }; diff --git a/dart/collision/CollisionGroupData.cpp b/dart/collision/CollisionGroupData.cpp deleted file mode 100644 index 873c99ce99b6b..0000000000000 --- a/dart/collision/CollisionGroupData.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/CollisionGroupData.h" - -#include - -namespace dart { -namespace collision { - -//============================================================================== -CollisionGroupData::CollisionGroupData( - CollisionDetector* collisionDetector, - CollisionGroup* parent) - : mCollisionDetector(collisionDetector), - mParent(parent) -{ - assert(mCollisionDetector); - assert(mParent); -} - -//============================================================================== -CollisionDetector* CollisionGroupData::getCollisionDetector() -{ - return mCollisionDetector; -} - -//============================================================================== -const CollisionDetector* CollisionGroupData::getCollisionDetector() const -{ - return mCollisionDetector; -} - -//============================================================================== -CollisionGroup* CollisionGroupData::getCollisionGroup() -{ - return mParent; -} - -//============================================================================== -const CollisionGroup* CollisionGroupData::getCollisionGroup() const -{ - return mParent; -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/CollisionGroupData.h b/dart/collision/CollisionGroupData.h deleted file mode 100644 index 5416e301a82ab..0000000000000 --- a/dart/collision/CollisionGroupData.h +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_COLLISIONGROUPDATA_H_ -#define DART_COLLISION_COLLISIONGROUPDATA_H_ - -#include -#include "dart/collision/SmartPointer.h" - -namespace dart { -namespace collision { - -class CollisionObject; - -class CollisionGroupData -{ -public: - - using CollisionObjectPtrs = std::vector; - - CollisionGroupData(CollisionDetector* collisionDetector, - CollisionGroup* parent); - - /// Initiate collision group data. This function should be called whenever - /// collision object is added or removed. - virtual void init() = 0; - - virtual void addCollisionObject(const CollisionObjectPtr& object, - bool init = true) = 0; - - virtual void addCollisionObjects(const CollisionObjectPtrs& objects, - bool init = true) = 0; - - virtual void removeCollisionObject(const CollisionObjectPtr& object, - bool init = true) = 0; - - virtual void removeAllCollisionObjects(bool init = true) = 0; - - /// Update engine data. This function will be called ahead of every collision - /// checking - virtual void update() = 0; - - virtual std::unique_ptr clone( - CollisionGroup* newCollisionGroup, - const CollisionObjectPtrs& collObjects) const = 0; - - CollisionDetector* getCollisionDetector(); - - const CollisionDetector* getCollisionDetector() const; - - CollisionGroup* getCollisionGroup(); - - const CollisionGroup* getCollisionGroup() const; - -protected: - - CollisionDetector* mCollisionDetector; - - CollisionGroup* mParent; - -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_COLLISIONGROUPDATA_H_ diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index d7b985da6e28b..ea981a0ce4eb2 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -37,7 +37,8 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObject.h" +#include "dart/dynamics/ShapeFrame.h" namespace dart { namespace collision { @@ -45,60 +46,75 @@ namespace collision { //============================================================================== CollisionObject::~CollisionObject() { -// mCollisionDetector->reclaimCollisionObject(this); - mCollisionDetector->reclaimCollisionObjectData(mEngineData.get()); + // Do nothing } //============================================================================== -CollisionDetector* CollisionObject::getCollisionDetector() const +CollisionDetector* CollisionObject::getCollisionDetector() { - return mCollisionDetector.get(); + return mCollisionDetector; } //============================================================================== -dynamics::ShapePtr CollisionObject::getShape() const +const CollisionDetector* CollisionObject::getCollisionDetector() const { - return mShape; + return mCollisionDetector; } -//============================================================================== -bool CollisionObject::detect(CollisionObject* other, - const Option& option, - Result& result) -{ - return mCollisionDetector->detect(this, other, option, result); -} +////============================================================================== +//const dynamics::ShapeFrame* CollisionObject::getShapeFrame() +//{ +// return mShapeFrame; +//} //============================================================================== -bool CollisionObject::detect(CollisionGroup* group, - const Option& option, Result& result) +const dynamics::ShapeFrame* CollisionObject::getShapeFrame() const { - return mCollisionDetector->detect(this, group, option, result); + return mShapeFrame; } +////============================================================================== +//dynamics::ShapePtr CollisionObject::getShape() +//{ +// return mShapeFrame->getShape(); +//} + //============================================================================== -CollisionObjectData* CollisionObject::getEngineData() const +dynamics::ConstShapePtr CollisionObject::getShape() const { - return mEngineData.get(); + return mShapeFrame->getShape(); } //============================================================================== -void CollisionObject::updateEngineData() +const Eigen::Isometry3d& CollisionObject::getTransform() const { - mEngineData->update(); + return mShapeFrame->getWorldTransform(); } +////============================================================================== +//bool CollisionObject::detect(CollisionObject* other, +// const Option& option, +// Result& result) +//{ +// return mCollisionDetector->detect(this, other, option, result); +//} + +////============================================================================== +//bool CollisionObject::detect(CollisionGroup* group, +// const Option& option, Result& result) +//{ +// return mCollisionDetector->detect(this, group, option, result); +//} + //============================================================================== CollisionObject::CollisionObject( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape) + CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame) : mCollisionDetector(collisionDetector), - mShape(shape), - mEngineData( - mCollisionDetector->createCollisionObjectData(this, mShape).release()) + mShapeFrame(shapeFrame) { assert(mCollisionDetector); - assert(mShape); + assert(mShapeFrame); } //============================================================================== @@ -124,55 +140,55 @@ bool CollisionObject::hasGroup(CollisionGroup* group) return std::find(mGroups.begin(), mGroups.end(), group) != mGroups.end(); } -//============================================================================== -std::shared_ptr FreeCollisionObject::create( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf) -{ - return collisionDetector->createCollisionObject( - shape, tf); -} - -//============================================================================== -void FreeCollisionObject::setTransform(const Eigen::Isometry3d& tf) -{ - mW = tf; -} - -//============================================================================== -void FreeCollisionObject::setRotation(const Eigen::Matrix3d& rotation) -{ - mW.linear() = rotation; -} - -//============================================================================== -void FreeCollisionObject::setTranslation(const Eigen::Vector3d& translation) -{ - mW.translation() = translation; -} - -//============================================================================== -const Eigen::Isometry3d FreeCollisionObject::getTransform() const -{ - return mW; -} - -//============================================================================== -bool FreeCollisionObject::isEqual(const CollisionObject* other) const -{ - return this == other; -} - -//============================================================================== -FreeCollisionObject::FreeCollisionObject( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf) - : CollisionObject(collisionDetector, shape), - mW(tf) -{ - // Do nothing -} +////============================================================================== +//std::shared_ptr FreeCollisionObject::create( +// const CollisionDetectorPtr& collisionDetector, +// const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf) +//{ +// return collisionDetector->createCollisionObject( +// shape, tf); +//} + +////============================================================================== +//void FreeCollisionObject::setTransform(const Eigen::Isometry3d& tf) +//{ +// mW = tf; +//} + +////============================================================================== +//void FreeCollisionObject::setRotation(const Eigen::Matrix3d& rotation) +//{ +// mW.linear() = rotation; +//} + +////============================================================================== +//void FreeCollisionObject::setTranslation(const Eigen::Vector3d& translation) +//{ +// mW.translation() = translation; +//} + +////============================================================================== +//const Eigen::Isometry3d FreeCollisionObject::getTransform() const +//{ +// return mW; +//} + +////============================================================================== +//bool FreeCollisionObject::isEqual(const CollisionObject* other) const +//{ +// return this == other; +//} + +////============================================================================== +//FreeCollisionObject::FreeCollisionObject( +// const CollisionDetectorPtr& collisionDetector, +// const dynamics::ShapePtr& shape, +// const Eigen::Isometry3d& tf) +// : CollisionObject(collisionDetector, shape), +// mW(tf) +//{ +// // Do nothing +//} } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 3b223cc51d960..1b7be5bbe35ea 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -41,11 +41,13 @@ #include "dart/collision/CollisionDetector.h" #include "dart/collision/SmartPointer.h" -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObject.h" +#include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { +// TODO: this should be in collision::detail namespace class CollisionObject { public: @@ -55,43 +57,49 @@ class CollisionObject virtual ~CollisionObject(); /// Return collision detection engine associated with this CollisionObject - CollisionDetector* getCollisionDetector() const; + CollisionDetector* getCollisionDetector(); - /// Return shape pointer that associated with this CollisionObject - dynamics::ShapePtr getShape() const; - // TODO(JS): Shape should be in common or math + /// Return collision detection engine associated with this CollisionObject + const CollisionDetector* getCollisionDetector() const; - /// Return the transformation of this CollisionObject in world coordinates - virtual const Eigen::Isometry3d getTransform() const = 0; +// /// Return the associated ShapeFrame +// dynamics::ShapeFrame* getShapeFrame(); + + /// Return the associated ShapeFrame + const dynamics::ShapeFrame* getShapeFrame() const; + +// /// Return the associated Shape +// dynamics::ShapePtr getShape(); - /// Return true if this CollisionObject is identical to other - virtual bool isEqual(const CollisionObject* other) const = 0; + /// Return the associated Shape + dynamics::ConstShapePtr getShape() const; + + /// Return the transformation of this CollisionObject in world coordinates + const Eigen::Isometry3d& getTransform() const; /// Perform collision detection with other CollisionObject. /// /// Return false if the engine type of the other CollisionObject is different /// from this CollisionObject engine. - bool detect(CollisionObject* other, const Option& option, Result& result); +// bool detect(CollisionObject* other, const Option& option, Result& result); /// Perform collision detection with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool detect(CollisionGroup* group, const Option& option, Result& result); - - /// Return the collision detection engine specific data of this - /// CollisionObject - CollisionObjectData* getEngineData() const; - - /// Update engine data. This function should be called before the collision - /// detection is performed by the engine in most cases. - void updateEngineData(); +// bool detect(CollisionGroup* group, const Option& option, Result& result); protected: /// Contructor - CollisionObject(const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape); + CollisionObject(CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame); + + /// Update the collision object of the collision detection engine. This + /// function will be called ahead of every collision checking by + /// CollisionGroup. + virtual void updateEngineData() = 0; + // TODO(JS): Rename void addGroup(CollisionGroup* group); @@ -102,13 +110,10 @@ class CollisionObject protected: /// Collision detector - CollisionDetectorPtr mCollisionDetector; + CollisionDetector* mCollisionDetector; - /// Shape - dynamics::ShapePtr mShape; - - /// Collision detection engine specific data - std::unique_ptr mEngineData; + /// ShapeFrame + const dynamics::ShapeFrame* mShapeFrame; std::vector mGroups; @@ -116,45 +121,45 @@ class CollisionObject /// FreeCollisionOjbect is a basic collision object whose transformation can be /// set freely. -class FreeCollisionObject : public CollisionObject -{ -public: +//class FreeCollisionObject : public CollisionObject +//{ +//public: - friend class CollisionDetector; +// friend class CollisionDetector; - static std::shared_ptr create( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); +// static std::shared_ptr create( +// const CollisionDetectorPtr& collisionDetector, +// const dynamics::ShapePtr& shape, +// const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); - /// Set world transformation of this FreeCollisionObject - void setTransform(const Eigen::Isometry3d& tf); +// /// Set world transformation of this FreeCollisionObject +// void setTransform(const Eigen::Isometry3d& tf); - /// Set world rotation of this FreeCollisionObject - void setRotation(const Eigen::Matrix3d& rotation); +// /// Set world rotation of this FreeCollisionObject +// void setRotation(const Eigen::Matrix3d& rotation); - /// Set world translation of this FreeCollisionObject - void setTranslation(const Eigen::Vector3d& translation); +// /// Set world translation of this FreeCollisionObject +// void setTranslation(const Eigen::Vector3d& translation); - /// Return world transformation of this FreeCollisionObject - const Eigen::Isometry3d getTransform() const override; +// /// Return world transformation of this FreeCollisionObject +// const Eigen::Isometry3d getTransform() const override; - // Documentation inherited - bool isEqual(const CollisionObject* other) const override; +// // Documentation inherited +// bool isEqual(const CollisionObject* other) const override; -protected: +//protected: - /// Constructor - FreeCollisionObject( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapePtr& shape, - const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); - // TODO(JS): change to engine pointer +// /// Constructor +// FreeCollisionObject( +// const CollisionDetectorPtr& collisionDetector, +// const dynamics::ShapePtr& shape, +// const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); +// // TODO(JS): change to engine pointer - /// Transformation in world coordinates - Eigen::Isometry3d mW; +// /// Transformation in world coordinates +// Eigen::Isometry3d mW; -}; +//}; } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObjectData.cpp b/dart/collision/CollisionObjectData.cpp deleted file mode 100644 index a26cc4f0bc51c..0000000000000 --- a/dart/collision/CollisionObjectData.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/CollisionObjectData.h" - -namespace dart { -namespace collision { - -//============================================================================== -CollisionObjectData::CollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent) - : mCollisionDetector(collisionDetector), - mParent(parent) -{ - assert(mCollisionDetector); - assert(mParent); -} - -//============================================================================== -CollisionDetector* CollisionObjectData::getCollisionDetector() -{ - return mCollisionDetector; -} - -//============================================================================== -const CollisionDetector* CollisionObjectData::getCollisionDetector() const -{ - return mCollisionDetector; -} - -//============================================================================== -CollisionObject* CollisionObjectData::getCollisionObject() -{ - return mParent; -} - -//============================================================================== -const CollisionObject* CollisionObjectData::getCollisionObject() const -{ - return mParent; -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/CollisionObjectData.h b/dart/collision/CollisionObjectData.h deleted file mode 100644 index 1839b9f5e1b15..0000000000000 --- a/dart/collision/CollisionObjectData.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_COLLISIONOBJECTDATA_H_ -#define DART_COLLISION_COLLISIONOBJECTDATA_H_ - -#include - -#include "dart/collision/SmartPointer.h" -#include "dart/dynamics/SmartPointer.h" - -namespace dart { -namespace collision { - -class CollisionObjectData -{ -public: - - CollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent); - - virtual void updateTransform(const Eigen::Isometry3d& tf) = 0; - - /// Update engine data. This function will be called ahead of every collision - /// checking - virtual void update() = 0; - // TODO(JS): reorganize updateTansform/updateShape/update - - CollisionDetector* getCollisionDetector(); - - const CollisionDetector* getCollisionDetector() const; - - CollisionObject* getCollisionObject(); - - const CollisionObject* getCollisionObject() const; - -protected: - - CollisionDetector* mCollisionDetector; - - CollisionObject* mParent; - -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_COLLISIONOBJECTDATA_H_ diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h index 7f5f88dfd4a3c..a2cee4585bda4 100644 --- a/dart/collision/SmartPointer.h +++ b/dart/collision/SmartPointer.h @@ -52,11 +52,7 @@ DART_COMMON_MAKE_SHARED_WEAK(DARTCollisionDetector) #endif DART_COMMON_MAKE_SHARED_WEAK(CollisionObject) -DART_COMMON_MAKE_SHARED_WEAK(CollisionObjectData) -DART_COMMON_MAKE_SHARED_WEAK(FreeCollisionObject) - DART_COMMON_MAKE_SHARED_WEAK(CollisionGroup) -DART_COMMON_MAKE_SHARED_WEAK(CollisionGroupData) } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index c0baa1e90f969..f0aa031c63cde 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -41,8 +41,9 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/bullet/BulletTypes.h" -#include "dart/collision/bullet/BulletCollisionObjectData.h" -#include "dart/collision/bullet/BulletCollisionGroupData.h" +#include "dart/collision/bullet/BulletCollisionObject.h" +#include "dart/collision/bullet/BulletCollisionGroup.h" +#include "dart/dynamics/ShapeFrame.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" #include "dart/dynamics/CylinderShape.h" @@ -83,9 +84,9 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback auto bulletCollObj1 = static_cast(proxy1->m_clientObject); - auto userData0 = static_cast( + auto userData0 = static_cast( bulletCollObj0->getUserPointer()); - auto userData1 = static_cast( + auto userData1 = static_cast( bulletCollObj1->getUserPointer()); auto collisionDetector = userData0->collisionDetector; @@ -151,62 +152,96 @@ const std::string& BulletCollisionDetector::getType() const } //============================================================================== -BulletCollisionObjectData* BulletCollisionDetector::findCollisionObjectData( +std::shared_ptr BulletCollisionDetector::createCollisionGroup() +{ + return std::make_shared(shared_from_this()); +} + +//============================================================================== +std::shared_ptr BulletCollisionDetector::createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) +{ + return std::make_shared(shared_from_this(), shapeFrame); +} + +//============================================================================== +std::shared_ptr BulletCollisionDetector::createCollisionGroup( + const std::vector& shapeFrames) +{ + return std::make_shared(shared_from_this(), + shapeFrames); +} + +//============================================================================== +std::unique_ptr +BulletCollisionDetector::createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) +{ + auto pack = claimBulletCollisionGeometry(shapeFrame->getShape()); + auto collObj = new BulletCollisionObject(this, shapeFrame, + pack.collisionShape.get()); + auto object = collObj->getBulletCollisionObject(); + + mBulletCollisionObjectMap[object] = collObj; + + return std::unique_ptr(collObj); +} + +//============================================================================== +BulletCollisionObject* BulletCollisionDetector::findCollisionObject( btCollisionObject* bulletCollObj) const { - auto search = mCollisionObjectMap.find(bulletCollObj); - if (mCollisionObjectMap.end() != search) + auto search = mBulletCollisionObjectMap.find(bulletCollObj); + if (mBulletCollisionObjectMap.end() != search) return search->second; else return nullptr; } //============================================================================== -CollisionObject* BulletCollisionDetector::findCollisionObject( - btCollisionObject* bulletCollObj) const +void BulletCollisionDetector::notifyDestroyingCollisionObject( + CollisionObject* collObj) { - auto data = findCollisionObjectData(bulletCollObj); + if (!collObj) + return; - if (data) - return data->getCollisionObject(); - else - return nullptr; + reclaimBulletCollisionGeometry(collObj->getShape()); + + auto casted = static_cast(collObj); + mBulletCollisionObjectMap.erase(casted->getBulletCollisionObject()); } //============================================================================== -std::unique_ptr -BulletCollisionDetector::createCollisionObjectData( - CollisionObject* parent, const dynamics::ShapePtr& shape) +BulletCollisionDetector::BulletCollsionPack +BulletCollisionDetector::claimBulletCollisionGeometry( + const dynamics::ConstShapePtr& shape) { - btCollisionShape* bulletCollGeom; + BulletCollsionPack pack; auto findResult = mShapeMap.find(shape); if (mShapeMap.end() != findResult) { - ShapeMapValue& pair = findResult->second; - BulletCollsionPack& pack = pair.first; - bulletCollGeom = pack.collisionShape.get(); - pair.second++; + auto& packAndCount = findResult->second; + + pack = packAndCount.first; + + auto& count = packAndCount.second; + assert(0u != count); + count++; } else { - BulletCollsionPack pack = createBulletCollisionShape(shape); - bulletCollGeom = pack.collisionShape.get(); - mShapeMap[shape] = std::make_pair(std::move(pack), 1u); + pack = createBulletCollisionShape(shape); + mShapeMap[shape] = std::make_pair(pack, 1u); } - return std::unique_ptr( - new BulletCollisionObjectData(this, parent, bulletCollGeom)); + return pack; } //============================================================================== -void BulletCollisionDetector::reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) +void BulletCollisionDetector::reclaimBulletCollisionGeometry( + const dynamics::ConstShapePtr& shape) { - // Retrieve associated shape - auto shape = collisionObjectData->getCollisionObject()->getShape(); - assert(shape); - auto findResult = mShapeMap.find(shape); assert(mShapeMap.end() != findResult); @@ -219,20 +254,10 @@ void BulletCollisionDetector::reclaimCollisionObjectData( } } -//============================================================================== -std::unique_ptr -BulletCollisionDetector::createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) -{ - return std::unique_ptr( - new BulletCollisionGroupData(this, parent, collObjects)); -} - //============================================================================== Contact convertContact(const btManifoldPoint& bulletManifoldPoint, - const BulletCollisionObjectData::UserData* userData1, - const BulletCollisionObjectData::UserData* userData2) + const BulletCollisionObject::UserData* userData1, + const BulletCollisionObject::UserData* userData2) { assert(userData1); assert(userData2); @@ -268,9 +293,9 @@ void convertContacts(btCollisionWorld* collWorld, Result& result) auto userPointer1 = bulletCollObj1->getUserPointer(); auto userDataA - = static_cast(userPointer1); + = static_cast(userPointer1); auto userDataB - = static_cast(userPointer0); + = static_cast(userPointer0); auto numContacts = contactManifold->getNumContacts(); @@ -285,82 +310,28 @@ void convertContacts(btCollisionWorld* collWorld, Result& result) //============================================================================== bool BulletCollisionDetector::detect( - CollisionObjectData* objectData1, - CollisionObjectData* objectData2, - const Option& /*option*/, Result& result) + CollisionGroup* group, const Option& option, Result& result) { result.contacts.clear(); - assert(objectData1->getCollisionDetector()->getType() - == BulletCollisionDetector::getTypeStatic()); - assert(objectData2->getCollisionDetector()->getType() - == BulletCollisionDetector::getTypeStatic()); - - auto castedData1 = static_cast(objectData1); - auto castedData2 = static_cast(objectData2); - - auto bulletCollObj1 = castedData1->getBulletCollisionObject(); - auto bulletCollObj2 = castedData2->getBulletCollisionObject(); - - if (!mBulletCollisionGroupForSinglePair) - mBulletCollisionGroupForSinglePair = createCollisionGroup(); + if (!group) + return false; - auto cb = BulletContactResultCallback(result); - auto collisionGroupData = static_cast( - mBulletCollisionGroupForSinglePair->getEngineData()); - auto bulletCollisionWorld = collisionGroupData->getBulletCollisionWorld(); - bulletCollisionWorld->contactPairTest(bulletCollObj1, bulletCollObj2, cb); - - return !result.contacts.empty(); -} - -//============================================================================== -bool BulletCollisionDetector::detect( - CollisionObjectData* objectData, - CollisionGroupData* groupData, - const Option& /*option*/, Result& result) -{ - result.contacts.clear(); - - assert(objectData); - assert(groupData); - assert(objectData->getCollisionDetector()->getType() - == BulletCollisionDetector::getTypeStatic()); - assert(groupData->getCollisionDetector()->getType() - == BulletCollisionDetector::getTypeStatic()); - - auto castedObjData = static_cast(objectData); - auto castedGrpData = static_cast(groupData); - - auto bulletCollObj = castedObjData->getBulletCollisionObject(); - auto bulletCollisionWorld = castedGrpData->getBulletCollisionWorld(); - - auto cb = BulletContactResultCallback(result); - bulletCollisionWorld->contactTest(bulletCollObj, cb); - - return !result.contacts.empty(); -} - -//============================================================================== -bool BulletCollisionDetector::detect( - CollisionGroupData* groupData, - const Option& option, Result& result) -{ - result.contacts.clear(); - - assert(groupData); - assert(groupData->getCollisionDetector()->getType() - == BulletCollisionDetector::getTypeStatic()); + if (group->getCollisionDetector()->getType() + != BulletCollisionDetector::getTypeStatic()) + { + return false; + } - auto castedData = static_cast(groupData); + group->update(); + auto castedData = static_cast(group); auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); - auto filterCallback = new BulletOverlapFilterCallback(option.collisionFilter.get()); - bulletPairCache->setOverlapFilterCallback(filterCallback); + bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); convertContacts(bulletCollisionWorld, result); @@ -370,25 +341,35 @@ bool BulletCollisionDetector::detect( //============================================================================== bool BulletCollisionDetector::detect( - CollisionGroupData* /*groupData1*/, - CollisionGroupData* /*groupData2*/, - const Option& /*option*/, Result& result) + CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) { result.contacts.clear(); -// assert(groupData1); -// assert(groupData2); -// assert(groupData1->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); -// assert(groupData2->getCollisionDetector()->getType() == BulletCollisionDetector::getTypeStatic()); + if (!group1 || !group2) + return false; + + if (group1->getCollisionDetector()->getType() + != BulletCollisionDetector::getTypeStatic()) + { + return false; + } + + if (group2->getCollisionDetector()->getType() + != BulletCollisionDetector::getTypeStatic()) + { + return false; + } -// auto castedData1 = static_cast(groupData1); -// auto castedData2 = static_cast(groupData2); + auto castedData1 = static_cast(group1); + auto castedData2 = static_cast(group2); -// auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); -// auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); + auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); + auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); // BulletCollisionData collData(&option, &result); // bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); + // TODO(JS) return !result.contacts.empty(); } @@ -455,7 +436,7 @@ BulletCollisionDetector::createSoftMesh(const aiMesh* mesh) //============================================================================== BulletCollisionDetector::BulletCollsionPack BulletCollisionDetector::createBulletCollisionShape( - const dynamics::ShapePtr& shape) + const dynamics::ConstShapePtr& shape) { using dynamics::Shape; using dynamics::BoxShape; @@ -473,7 +454,7 @@ BulletCollisionDetector::createBulletCollisionShape( { case Shape::BOX: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); const auto box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); @@ -484,9 +465,9 @@ BulletCollisionDetector::createBulletCollisionShape( } case Shape::ELLIPSOID: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); - const auto ellipsoid = static_cast(shape.get()); + const auto ellipsoid = static_cast(shape.get()); const Eigen::Vector3d& size = ellipsoid->getSize(); if (ellipsoid->isSphere()) @@ -502,7 +483,7 @@ BulletCollisionDetector::createBulletCollisionShape( } case Shape::CYLINDER: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); const auto cylinder = static_cast(shape.get()); const auto radius = cylinder->getRadius(); @@ -515,9 +496,9 @@ BulletCollisionDetector::createBulletCollisionShape( } case Shape::PLANE: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); - const auto plane = static_cast(shape.get()); + const auto plane = static_cast(shape.get()); const Eigen::Vector3d normal = plane->getNormal(); const double offset = plane->getOffset(); @@ -528,9 +509,9 @@ BulletCollisionDetector::createBulletCollisionShape( } case Shape::MESH: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); - const auto shapeMesh = static_cast(shape.get()); + const auto shapeMesh = static_cast(shape.get()); const auto scale = shapeMesh->getScale(); const auto mesh = shapeMesh->getMesh(); @@ -540,9 +521,9 @@ BulletCollisionDetector::createBulletCollisionShape( } case Shape::SOFT_MESH: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); - const auto softMeshShape = static_cast(shape.get()); + const auto softMeshShape = static_cast(shape.get()); const auto mesh = softMeshShape->getAssimpMesh(); pack = createSoftMesh(mesh); @@ -597,9 +578,9 @@ btScalar BulletContactResultCallback::addSingleResult( auto userPointer1 = colObj1Wrap->getCollisionObject()->getUserPointer(); auto userDataA - = static_cast(userPointer1); + = static_cast(userPointer1); auto userDataB - = static_cast(userPointer0); + = static_cast(userPointer0); mResult.contacts.push_back(convertContact(cp, userDataA, userDataB)); diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 3890927140b98..c0def1074edd4 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -45,8 +45,7 @@ namespace dart { namespace collision { -class CollisionGroup; -class BulletCollisionObjectData; +class BulletCollisionObject; class BulletCollisionDetector : public CollisionDetector { @@ -65,75 +64,79 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited const std::string& getType() const override; - using CollisionDetector::detect; + // Documentation inherited + std::shared_ptr createCollisionGroup() override; - BulletCollisionObjectData* findCollisionObjectData( - btCollisionObject* bulletCollObj) const; + // Documentation inherited + std::shared_ptr createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) override; - CollisionObject* findCollisionObject( - btCollisionObject* bulletCollObj) const; + // Documentation inherited + std::shared_ptr createCollisionGroup( + const std::vector& shapeFrames) override; + + using CollisionDetector::detect; protected: + struct BulletCollsionPack + { + std::shared_ptr collisionShape; + std::shared_ptr triMesh; + // TODO(JS): change to unique_ptr + }; + /// Constructor BulletCollisionDetector() = default; // Documentation inherited - std::unique_ptr createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) override; + std::unique_ptr createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) override; - // Documentation inherited - void reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) override; +public: - // Documentation inherited - std::unique_ptr createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) override; + BulletCollisionObject* findCollisionObject( + btCollisionObject* bulletCollObj) const; + +protected: // Documentation inherited - bool detect(CollisionObjectData* object1, CollisionObjectData* object2, - const Option& option, Result& result) override; + void notifyDestroyingCollisionObject(CollisionObject* collObj) override; + + /// + BulletCollsionPack claimBulletCollisionGeometry( + const dynamics::ConstShapePtr& shape); // Documentation inherited - bool detect(CollisionObjectData* object, CollisionGroupData* group, - const Option& option, Result& result) override; + void reclaimBulletCollisionGeometry(const dynamics::ConstShapePtr& shape); // Documentation inherited - bool detect(CollisionGroupData* group, + bool detect(CollisionGroup* group, const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) override; -protected: - - struct BulletCollsionPack - { - std::unique_ptr collisionShape; - std::unique_ptr triMesh; - }; - BulletCollsionPack createMesh( const Eigen::Vector3d& scale, const aiScene* mesh); BulletCollsionPack createSoftMesh(const aiMesh* mesh); BulletCollsionPack createBulletCollisionShape( - const dynamics::ShapePtr& shape); + const dynamics::ConstShapePtr& shape); protected: using ShapeMapValue = std::pair; - std::map mShapeMap; + std::map mShapeMap; std::map mCollisionObjectMap; + BulletCollisionObject*> mBulletCollisionObjectMap; std::shared_ptr mBulletCollisionGroupForSinglePair; + // TODO(JS): should be unique_ptr }; diff --git a/dart/collision/bullet/BulletCollisionGroupData.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp similarity index 54% rename from dart/collision/bullet/BulletCollisionGroupData.cpp rename to dart/collision/bullet/BulletCollisionGroup.cpp index c9e8ac657a326..f1489a2511f81 100644 --- a/dart/collision/bullet/BulletCollisionGroupData.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -34,20 +34,18 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/bullet/BulletCollisionGroupData.h" +#include "dart/collision/bullet/BulletCollisionGroup.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/bullet/BulletCollisionObjectData.h" +#include "dart/collision/bullet/BulletCollisionObject.h" namespace dart { namespace collision { //============================================================================== -BulletCollisionGroupData::BulletCollisionGroupData( - CollisionDetector* collisionDetector, - CollisionGroup* parent, - const BulletCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(collisionDetector, parent), +BulletCollisionGroup::BulletCollisionGroup( + const CollisionDetectorPtr& collisionDetector) + : CollisionGroup(collisionDetector), mBulletProadphaseAlg( new btDbvtBroadphase()), mBulletCollisionConfiguration( @@ -59,21 +57,60 @@ BulletCollisionGroupData::BulletCollisionGroupData( mBulletProadphaseAlg.get(), mBulletCollisionConfiguration.get())) { - addCollisionObjects(collObjects, true); + assert(mCollisionDetector); } //============================================================================== -std::unique_ptr BulletCollisionGroupData::clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const +BulletCollisionGroup::BulletCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame) + : CollisionGroup(collisionDetector), + mBulletProadphaseAlg( + new btDbvtBroadphase()), + mBulletCollisionConfiguration( + new btDefaultCollisionConfiguration()), + mBulletDispatcher( + new btCollisionDispatcher(mBulletCollisionConfiguration.get())), + mBulletCollisionWorld( + new btCollisionWorld(mBulletDispatcher.get(), + mBulletProadphaseAlg.get(), + mBulletCollisionConfiguration.get())) +{ + assert(mCollisionDetector); + assert(shapeFrame); + + addShapeFrame(shapeFrame); +} + +//============================================================================== +BulletCollisionGroup::BulletCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames) + : CollisionGroup(collisionDetector), + mBulletProadphaseAlg( + new btDbvtBroadphase()), + mBulletCollisionConfiguration( + new btDefaultCollisionConfiguration()), + mBulletDispatcher( + new btCollisionDispatcher(mBulletCollisionConfiguration.get())), + mBulletCollisionWorld( + new btCollisionWorld(mBulletDispatcher.get(), + mBulletProadphaseAlg.get(), + mBulletCollisionConfiguration.get())) +{ + assert(mCollisionDetector); + + addShapeFrames(shapeFrames); +} + +//============================================================================== +BulletCollisionGroup::~BulletCollisionGroup() { - return std::unique_ptr( - new BulletCollisionGroupData(mCollisionDetector, newParent, - collObjects)); + removeAllShapeFrames(); } //============================================================================== -void BulletCollisionGroupData::init() +void BulletCollisionGroup::initializeEngineData() { btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); // dispatchInfo.m_timeStep = 0.001; @@ -81,58 +118,53 @@ void BulletCollisionGroupData::init() } //============================================================================== -void BulletCollisionGroupData::addCollisionObject( - const CollisionObjectPtr& object, const bool init) +void BulletCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) { - auto data = static_cast(object->getEngineData()); - mBulletCollisionWorld->addCollisionObject(data->getBulletCollisionObject()); + auto casted = static_cast(object); - if (init) - this->init(); + mBulletCollisionWorld->addCollisionObject(casted->getBulletCollisionObject()); + + this->initializeEngineData(); } //============================================================================== -void BulletCollisionGroupData::addCollisionObjects( - const BulletCollisionGroupData::CollisionObjectPtrs& collObjects, - const bool init) +void BulletCollisionGroup::addCollisionObjectsToEngine( + const std::vector& collObjects) { for (auto collObj : collObjects) { - auto data = static_cast( - collObj->getEngineData()); + auto casted = static_cast(collObj); - mBulletCollisionWorld->addCollisionObject(data->getBulletCollisionObject()); + mBulletCollisionWorld->addCollisionObject( + casted->getBulletCollisionObject()); } - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void BulletCollisionGroupData::removeCollisionObject( - const CollisionObjectPtr& object, const bool init) +void BulletCollisionGroup::removeCollisionObjectFromEngine( + CollisionObject* object) { - auto data = static_cast(object->getEngineData()); + auto casted = static_cast(object); + mBulletCollisionWorld->removeCollisionObject( - data->getBulletCollisionObject()); + casted->getBulletCollisionObject()); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void BulletCollisionGroupData::removeAllCollisionObjects(bool init) +void BulletCollisionGroup::removeAllCollisionObjectsFromEngine() { - auto collisionObjects = mParent->getCollisionObjects(); - for (auto collisionObject : collisionObjects) - removeCollisionObject(collisionObject, false); + for (const auto& collisionObject : getCollisionObjects()) + removeCollisionObjectFromEngine(collisionObject); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void BulletCollisionGroupData::update() +void BulletCollisionGroup::updateEngineData() { // Setting up broadphase collision detection options btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); @@ -143,7 +175,7 @@ void BulletCollisionGroupData::update() } //============================================================================== -btCollisionWorld* BulletCollisionGroupData::getBulletCollisionWorld() const +btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() const { return mBulletCollisionWorld.get(); } diff --git a/dart/collision/bullet/BulletCollisionGroupData.h b/dart/collision/bullet/BulletCollisionGroup.h similarity index 71% rename from dart/collision/bullet/BulletCollisionGroupData.h rename to dart/collision/bullet/BulletCollisionGroup.h index 16dac3e730ff7..92fe69666d476 100644 --- a/dart/collision/bullet/BulletCollisionGroupData.h +++ b/dart/collision/bullet/BulletCollisionGroup.h @@ -34,54 +34,61 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONGROUPDATA_H_ -#define DART_COLLISION_BULLET_BULLETCOLLISIONGROUPDATA_H_ +#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONGROUP_H_ +#define DART_COLLISION_BULLET_BULLETCOLLISIONGROUP_H_ #include #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { -class BulletCollisionGroupData : public CollisionGroupData +class BulletCollisionGroup : public CollisionGroup { public: + friend class BulletCollisionDetector; + using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - BulletCollisionGroupData( - CollisionDetector* collisionDetector, - CollisionGroup* parent, - const CollisionObjects& collObjects = CollisionObjects()); + BulletCollisionGroup( + const CollisionDetectorPtr& collisionDetector); - // Documentation inherited - std::unique_ptr clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const override; + /// Constructor + BulletCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame); + + /// Constructor + BulletCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames); + + /// Destructor + virtual ~BulletCollisionGroup(); + +protected: // Documentation inherited - void init() override; + void initializeEngineData() override; // Documentation inherited - void addCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void addCollisionObjects(const CollisionObjectPtrs& collObjects, - const bool init) override; + void addCollisionObjectsToEngine( + const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjects(bool init) override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited - void update() override; + void updateEngineData() override; /// Return Bullet collision world btCollisionWorld* getBulletCollisionWorld() const; @@ -104,4 +111,4 @@ class BulletCollisionGroupData : public CollisionGroupData } // namespace collision } // namespace dart -#endif // DART_COLLISION_BULLET_BULLETCOLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_BULLET_BULLETCOLLISIONGROUP_H_ diff --git a/dart/collision/bullet/BulletCollisionObjectData.cpp b/dart/collision/bullet/BulletCollisionObject.cpp similarity index 81% rename from dart/collision/bullet/BulletCollisionObjectData.cpp rename to dart/collision/bullet/BulletCollisionObject.cpp index 328b6b5ab8983..25de64b4c6f98 100644 --- a/dart/collision/bullet/BulletCollisionObjectData.cpp +++ b/dart/collision/bullet/BulletCollisionObject.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/bullet/BulletCollisionObjectData.h" +#include "dart/collision/bullet/BulletCollisionObject.h" #include "dart/common/Console.h" #include "dart/collision/bullet/BulletTypes.h" @@ -42,13 +42,14 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/bullet/BulletTypes.h" #include "dart/dynamics/Shape.h" +#include "dart/dynamics/ShapeFrame.h" #include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { //============================================================================== -BulletCollisionObjectData::UserData::UserData( +BulletCollisionObject::UserData::UserData( CollisionObject* collisionObject, CollisionDetector* collisionDetector, CollisionGroup* collisionGroup) @@ -60,30 +61,18 @@ BulletCollisionObjectData::UserData::UserData( } //============================================================================== -void BulletCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) -{ - mBulletCollisionObject->setWorldTransform(convertTransform(tf)); -} - -//============================================================================== -void BulletCollisionObjectData::update() -{ - updateTransform(mParent->getTransform()); -} - -//============================================================================== -btCollisionObject* BulletCollisionObjectData::getBulletCollisionObject() const +btCollisionObject* BulletCollisionObject::getBulletCollisionObject() const { return mBulletCollisionObject.get(); } //============================================================================== -BulletCollisionObjectData::BulletCollisionObjectData( +BulletCollisionObject::BulletCollisionObject( CollisionDetector* collisionDetector, - CollisionObject* parent, + const dynamics::ShapeFrame* shapeFrame, btCollisionShape* bulletCollisionShape) - : CollisionObjectData(collisionDetector, parent), - mBulletCollisionObjectUserData(new UserData(mParent, mCollisionDetector, + : CollisionObject(collisionDetector, shapeFrame), + mBulletCollisionObjectUserData(new UserData(this, mCollisionDetector, nullptr)), mBulletCollisionObject(new btCollisionObject()) { @@ -91,5 +80,12 @@ BulletCollisionObjectData::BulletCollisionObjectData( mBulletCollisionObject->setUserPointer(mBulletCollisionObjectUserData.get()); } +//============================================================================== +void BulletCollisionObject::updateEngineData() +{ + mBulletCollisionObject->setWorldTransform( + convertTransform(mShapeFrame->getWorldTransform())); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/bullet/BulletCollisionObjectData.h b/dart/collision/bullet/BulletCollisionObject.h similarity index 82% rename from dart/collision/bullet/BulletCollisionObjectData.h rename to dart/collision/bullet/BulletCollisionObject.h index 2408bc6731b27..7c5795a3dd723 100644 --- a/dart/collision/bullet/BulletCollisionObjectData.h +++ b/dart/collision/bullet/BulletCollisionObject.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONOBJECTDATA_H_ -#define DART_COLLISION_BULLET_BULLETCOLLISIONOBJECTDATA_H_ +#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONOBJECT_H_ +#define DART_COLLISION_BULLET_BULLETCOLLISIONOBJECT_H_ #include #include @@ -43,14 +43,14 @@ #include #include #include -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObject.h" namespace dart { namespace collision { class CollisionObject; -class BulletCollisionObjectData : public CollisionObjectData +class BulletCollisionObject : public CollisionObject { public: @@ -68,21 +68,18 @@ class BulletCollisionObjectData : public CollisionObjectData friend class BulletCollisionDetector; - // Documentation inherited - void updateTransform(const Eigen::Isometry3d& tf) override; - - // Documentation inherited - void update() override; - /// Return FCL collision object btCollisionObject* getBulletCollisionObject() const; protected: /// Constructor - BulletCollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent, - btCollisionShape* bulletCollisionShape); + BulletCollisionObject(CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame, + btCollisionShape* bulletCollisionShape); + + // Documentation inherited + void updateEngineData() override; protected: @@ -97,4 +94,4 @@ class BulletCollisionObjectData : public CollisionObjectData } // namespace collision } // namespace dart -#endif // DART_COLLISION_BULLET_BULLETCOLLISIONOBJECTDATA_H_ +#endif // DART_COLLISION_BULLET_BULLETCOLLISIONOBJECT_H_ diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 7351b8d9c6b98..912f711142f3e 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -39,8 +39,8 @@ #include #include "dart/collision/CollisionObject.h" #include "dart/collision/dart/DARTCollide.h" -#include "dart/collision/dart/DARTCollisionObjectData.h" -#include "dart/collision/dart/DARTCollisionGroupData.h" +#include "dart/collision/dart/DARTCollisionObject.h" +#include "dart/collision/dart/DARTCollisionGroup.h" namespace dart { namespace collision { @@ -80,102 +80,74 @@ const std::string& DARTCollisionDetector::getType() const } //============================================================================== -std::unique_ptr -DARTCollisionDetector::createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& /*shape*/) +std::shared_ptr DARTCollisionDetector::createCollisionGroup() { - return std::unique_ptr( - new DARTCollisionObjectData(this, parent)); + return std::make_shared(shared_from_this()); } //============================================================================== -void DARTCollisionDetector::reclaimCollisionObjectData( - CollisionObjectData* /*collisionObjectData*/) +std::shared_ptr DARTCollisionDetector::createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) { - // Do nothing + return std::make_shared(shared_from_this(), shapeFrame); } //============================================================================== -std::unique_ptr -DARTCollisionDetector::createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) +std::shared_ptr DARTCollisionDetector::createCollisionGroup( + const std::vector& shapeFrames) { - return std::unique_ptr( - new DARTCollisionGroupData(this, parent, collObjects)); + return std::make_shared(shared_from_this(), shapeFrames); } //============================================================================== -bool DARTCollisionDetector::detect( - CollisionObjectData* objectData1, - CollisionObjectData* objectData2, - const Option& /*option*/, Result& result) +std::unique_ptr DARTCollisionDetector::createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { - result.contacts.clear(); + auto collObj = new DARTCollisionObject(this, shapeFrame); - assert(objectData1->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); - assert(objectData2->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); + mDARTCollisionObjects.push_back(collObj); - auto collObj1 = objectData1->getCollisionObject(); - auto collObj2 = objectData2->getCollisionObject(); - - return checkPair(collObj1, collObj2, result); + return std::unique_ptr(collObj); } //============================================================================== -bool DARTCollisionDetector::detect( - CollisionObjectData* objectData, - CollisionGroupData* groupData, - const Option& /*option*/, Result& result) +void DARTCollisionDetector::notifyDestroyingCollisionObject( + CollisionObject* collObj) { - result.contacts.clear(); - - assert(objectData); - assert(groupData); - assert(objectData->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); - assert(groupData->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); - - auto collObj1 = objectData->getCollisionObject(); - auto collGrp = groupData->getCollisionGroup(); - auto collObjs2 = collGrp->getCollisionObjects(); - - for (auto collObj2 : collObjs2) - checkPair(collObj1, collObj2.get(), result); + if (!collObj) + return; - return !result.contacts.empty(); + auto casted = static_cast(collObj); + mDARTCollisionObjects.erase( + std::remove(mDARTCollisionObjects.begin(), mDARTCollisionObjects.end(), + casted), mDARTCollisionObjects.end()); } //============================================================================== bool DARTCollisionDetector::detect( - CollisionGroupData* groupData, + CollisionGroup* group, const Option& /*option*/, Result& result) { result.contacts.clear(); - assert(groupData); - assert(groupData->getCollisionDetector()->getType() + assert(group); + assert(group->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); - auto collGrp = groupData->getCollisionGroup(); - auto collObjs = collGrp->getCollisionObjects(); + auto objects = group->getCollisionObjects(); - if (collObjs.empty()) + if (objects.empty()) return false; - for (auto i = 0u; i < collObjs.size() - 1; ++i) + for (auto i = 0u; i < objects.size() - 1; ++i) { - auto collObj1 = collObjs[i]; + auto collObj1 = objects[i]; - for (auto j = i + 1u; j < collObjs.size(); ++j) + for (auto j = i + 1u; j < objects.size(); ++j) { - auto collObj2 = collObjs[j]; + auto collObj2 = objects[j]; - checkPair(collObj1.get(), collObj2.get(), result); + checkPair(collObj1, collObj2, result); } } @@ -184,37 +156,34 @@ bool DARTCollisionDetector::detect( //============================================================================== bool DARTCollisionDetector::detect( - CollisionGroupData* groupData1, - CollisionGroupData* groupData2, + CollisionGroup* group1, + CollisionGroup* group2, const Option& /*option*/, Result& result) { result.contacts.clear(); - assert(groupData1); - assert(groupData2); - assert(groupData1->getCollisionDetector()->getType() + assert(group1); + assert(group2); + assert(group1->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); - assert(groupData2->getCollisionDetector()->getType() + assert(group2->getCollisionDetector()->getType() == DARTCollisionDetector::getTypeStatic()); - auto collGrp1 = groupData1->getCollisionGroup(); - auto collGrp2 = groupData2->getCollisionGroup(); - - auto collObjs1 = collGrp1->getCollisionObjects(); - auto collObjs2 = collGrp2->getCollisionObjects(); + auto objects1 = group1->getCollisionObjects(); + auto objects2 = group2->getCollisionObjects(); - if (collObjs1.empty() || collObjs2.empty()) + if (objects1.empty() || objects2.empty()) return false; - for (auto i = 0u; i < collObjs1.size(); ++i) + for (auto i = 0u; i < objects1.size(); ++i) { - auto collObj1 = collObjs1[i]; + auto collObj1 = objects1[i]; - for (auto j = 0u; j < collObjs2.size(); ++j) + for (auto j = 0u; j < objects2.size(); ++j) { - auto collObj2 = collObjs2[j]; + auto collObj2 = objects2[j]; - checkPair(collObj1.get(), collObj2.get(), result); + checkPair(collObj1, collObj2, result); } } diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 4b15a8d4b2784..08b1cd0d2fe93 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -38,12 +38,13 @@ #define DART_COLLISION_DART_DARTCOLLISIONDETECTOR_H_ #include - #include "dart/collision/CollisionDetector.h" namespace dart { namespace collision { +class DARTCollisionObject; + class DARTCollisionDetector : public CollisionDetector { public: @@ -56,6 +57,17 @@ class DARTCollisionDetector : public CollisionDetector // Documentation inherited const std::string& getType() const override; + // Documentation inherited + std::shared_ptr createCollisionGroup() override; + + // Documentation inherited + std::shared_ptr createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) override; + + // Documentation inherited + std::shared_ptr createCollisionGroup( + const std::vector& shapeFrames) override; + using CollisionDetector::detect; protected: @@ -64,34 +76,23 @@ class DARTCollisionDetector : public CollisionDetector DARTCollisionDetector() = default; // Documentation inherited - std::unique_ptr createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) override; + std::unique_ptr createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) override; // Documentation inherited - void reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) override; + void notifyDestroyingCollisionObject(CollisionObject* collObj) override; // Documentation inherited - std::unique_ptr createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) override; - - // Documentation inherited - bool detect(CollisionObjectData* object1, CollisionObjectData* object2, + bool detect(CollisionGroup* group, const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionObjectData* object, CollisionGroupData* group, + bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) override; - // Documentation inherited - bool detect(CollisionGroupData* group, - const Option& option, Result& result) override; +protected: - // Documentation inherited - bool detect(CollisionGroupData* group1, CollisionGroupData* group2, - const Option& option, Result& result) override; + std::vector mDARTCollisionObjects; }; diff --git a/dart/collision/dart/DARTCollisionGroupData.cpp b/dart/collision/dart/DARTCollisionGroup.cpp similarity index 65% rename from dart/collision/dart/DARTCollisionGroupData.cpp rename to dart/collision/dart/DARTCollisionGroup.cpp index b4eba42c6f0c0..17edf49af4362 100644 --- a/dart/collision/dart/DARTCollisionGroupData.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/dart/DARTCollisionGroupData.h" +#include "dart/collision/dart/DARTCollisionGroup.h" #include "dart/collision/CollisionObject.h" @@ -42,60 +42,76 @@ namespace dart { namespace collision { //============================================================================== -DARTCollisionGroupData::DARTCollisionGroupData( - CollisionDetector* collisionDetector, - CollisionGroup* parent, - const CollisionObjectPtrs& /*collObjects*/) - : CollisionGroupData(collisionDetector, parent) +DARTCollisionGroup::DARTCollisionGroup( + const CollisionDetectorPtr& collisionDetector) + : CollisionGroup(collisionDetector) { - // Do nothing + assert(mCollisionDetector); +} + +//============================================================================== +DARTCollisionGroup::DARTCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame) + : CollisionGroup(collisionDetector) +{ + assert(mCollisionDetector); + assert(shapeFrame); + + addShapeFrame(shapeFrame); +} + +//============================================================================== +DARTCollisionGroup::DARTCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames) + : CollisionGroup(collisionDetector) +{ + assert(mCollisionDetector); + + addShapeFrames(shapeFrames); } //============================================================================== -std::unique_ptr -DARTCollisionGroupData::clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const +DARTCollisionGroup::~DARTCollisionGroup() { - return std::unique_ptr( - new DARTCollisionGroupData(mCollisionDetector, newParent, collObjects)); + removeAllShapeFrames(); } //============================================================================== -void DARTCollisionGroupData::init() +void DARTCollisionGroup::initializeEngineData() { // Do nothing } //============================================================================== -void DARTCollisionGroupData::addCollisionObject( - const CollisionObjectPtr& /*object*/, const bool /*init*/) +void DARTCollisionGroup::addCollisionObjectToEngine(CollisionObject* /*object*/) { // Do nothing } //============================================================================== -void DARTCollisionGroupData::addCollisionObjects( - const CollisionObjectPtrs& /*collObjects*/, const bool /*init*/) +void DARTCollisionGroup::addCollisionObjectsToEngine( + const std::vector& /*collObjects*/) { // Do nothing } //============================================================================== -void DARTCollisionGroupData::removeCollisionObject( - const CollisionObjectPtr& /*object*/, const bool /*init*/) +void DARTCollisionGroup::removeCollisionObjectFromEngine( + CollisionObject* /*object*/) { // Do nothing } //============================================================================== -void DARTCollisionGroupData::removeAllCollisionObjects(bool /*init*/) +void DARTCollisionGroup::removeAllCollisionObjectsFromEngine() { // Do nothing } //============================================================================== -void DARTCollisionGroupData::update() +void DARTCollisionGroup::updateEngineData() { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionGroupData.h b/dart/collision/dart/DARTCollisionGroup.h similarity index 66% rename from dart/collision/dart/DARTCollisionGroupData.h rename to dart/collision/dart/DARTCollisionGroup.h index 277df969b8303..62b0840a84acc 100644 --- a/dart/collision/dart/DARTCollisionGroupData.h +++ b/dart/collision/dart/DARTCollisionGroup.h @@ -34,55 +34,59 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_DART_DARTCOLLISIONGROUPDATA_H_ -#define DART_COLLISION_DART_DARTCOLLISIONGROUPDATA_H_ +#ifndef DART_COLLISION_DART_DARTCOLLISIONGROUP_H_ +#define DART_COLLISION_DART_DARTCOLLISIONGROUP_H_ #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { class DARTCollisionObject; -class DARTCollisionGroupData : public CollisionGroupData +class DARTCollisionGroup : public CollisionGroup { public: /// Constructor - DARTCollisionGroupData(CollisionDetector* collisionDetector, - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects); + DARTCollisionGroup(const CollisionDetectorPtr& collisionDetector); - // Documentation inherited - std::unique_ptr clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const override; + /// Constructor + DARTCollisionGroup(const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame); + + /// Constructor + DARTCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames); + + /// Destructor + virtual ~DARTCollisionGroup(); + +protected: // Documentation inherited - void init() override; + void initializeEngineData() override; // Documentation inherited - void addCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void addCollisionObjects(const CollisionObjectPtrs& collObjects, - const bool init) override; + void addCollisionObjectsToEngine( + const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjects(bool init) override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited - void update() override; + void updateEngineData() override; }; } // namespace collision } // namespace dart -#endif // DART_COLLISION_DART_DARTCOLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_DART_DARTCOLLISIONGROUP_H_ diff --git a/dart/collision/dart/DARTCollisionObjectData.cpp b/dart/collision/dart/DARTCollisionObject.cpp similarity index 83% rename from dart/collision/dart/DARTCollisionObjectData.cpp rename to dart/collision/dart/DARTCollisionObject.cpp index dafa06cdabc88..447e3eb9c50d1 100644 --- a/dart/collision/dart/DARTCollisionObjectData.cpp +++ b/dart/collision/dart/DARTCollisionObject.cpp @@ -34,28 +34,22 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/dart/DARTCollisionObjectData.h" +#include "dart/collision/dart/DARTCollisionObject.h" namespace dart { namespace collision { //============================================================================== -void DARTCollisionObjectData::updateTransform(const Eigen::Isometry3d& /*tf*/) -{ - // Do nothing -} - -//============================================================================== -void DARTCollisionObjectData::update() +DARTCollisionObject::DARTCollisionObject( + CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame) + : CollisionObject(collisionDetector, shapeFrame) { // Do nothing } //============================================================================== -DARTCollisionObjectData::DARTCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent) - : CollisionObjectData(collisionDetector, parent) +void DARTCollisionObject::updateEngineData() { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionObjectData.h b/dart/collision/dart/DARTCollisionObject.h similarity index 79% rename from dart/collision/dart/DARTCollisionObjectData.h rename to dart/collision/dart/DARTCollisionObject.h index eea029059d315..dd8167644f47e 100644 --- a/dart/collision/dart/DARTCollisionObjectData.h +++ b/dart/collision/dart/DARTCollisionObject.h @@ -34,38 +34,35 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ -#define DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ +#ifndef DART_COLLISION_DART_DARTCOLLISIONOBJECT_H_ +#define DART_COLLISION_DART_DARTCOLLISIONOBJECT_H_ #include -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObject.h" namespace dart { namespace collision { class CollisionObject; -class DARTCollisionObjectData : public CollisionObjectData +class DARTCollisionObject : public CollisionObject { public: friend class DARTCollisionDetector; - // Documentation inherited - void updateTransform(const Eigen::Isometry3d& tf) override; - - // Documentation inherited - void update() override; - protected: /// Constructor - DARTCollisionObjectData(CollisionDetector* collisionDetector, - CollisionObject* parent); + DARTCollisionObject(CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame); + + // Documentation inherited + void updateEngineData() override; }; } // namespace collision } // namespace dart -#endif // DART_COLLISION_DART_DARTCOLLISIONOBJECTDATA_H_ +#endif // DART_COLLISION_DART_DARTCOLLISIONOBJECT_H_ diff --git a/dart/collision/detail/CollisionDetector.h b/dart/collision/detail/CollisionDetector.h index d714aac5ccf76..fee63aaa696ce 100644 --- a/dart/collision/detail/CollisionDetector.h +++ b/dart/collision/detail/CollisionDetector.h @@ -42,27 +42,7 @@ namespace dart { namespace collision { -//============================================================================== -template -std::shared_ptr CollisionDetector::createCollisionObject( - const dynamics::ShapePtr& shape, const Args&... args) -{ - auto newCollisionObject = std::shared_ptr( - new CollisionObjectType(shared_from_this(), shape, args...)); - for (auto collisionObject : mCollisionObjects) - { - auto locked = collisionObject.lock(); - assert(locked); - - if (newCollisionObject->isEqual(locked.get())) - return std::static_pointer_cast(locked); - } - - mCollisionObjects.push_back(newCollisionObject); - - return newCollisionObject; -} } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index b252c1bf5b8f3..2ee026ce665cb 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -50,8 +50,9 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" -#include "dart/collision/fcl/FCLCollisionGroupData.h" +#include "dart/collision/fcl/FCLCollisionObject.h" +#include "dart/collision/fcl/FCLCollisionGroup.h" +#include "dart/dynamics/ShapeFrame.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" @@ -537,7 +538,7 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) //============================================================================== boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ShapePtr& shape) + const dynamics::ConstShapePtr& shape) { using dynamics::Shape; using dynamics::BoxShape; @@ -553,18 +554,22 @@ boost::shared_ptr createFCLCollisionGeometry( { case Shape::BOX: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); auto box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset(createCube(size[0], size[1], size[2])); +#else + fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); +#endif break; } case Shape::ELLIPSOID: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); auto ellipsoid = static_cast(shape.get()); const Eigen::Vector3d& size = ellipsoid->getSize(); @@ -575,40 +580,61 @@ boost::shared_ptr createFCLCollisionGeometry( } else { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset( createEllipsoid(size[0], size[1], size[2])); +#else + fclCollGeom.reset( + new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); +#endif } break; } case Shape::CYLINDER: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); const auto cylinder = static_cast(shape.get()); const auto radius = cylinder->getRadius(); const auto height = cylinder->getHeight(); +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) fclCollGeom.reset(createCylinder( radius, radius, height, 16, 16)); +#else + fclCollGeom.reset(new fcl::Cylinder(radius, height)); +#endif + // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 + // returns single contact point for cylinder yet. break; } case Shape::PLANE: { +#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " << "FCLCollisionDetector. We create a thin box mesh insted, where " << "the size is [1000 0 1000].\n"; +#else + + assert(dynamic_cast(shape.get())); + auto plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + fclCollGeom.reset( + new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); +#endif break; } case Shape::MESH: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); - auto shapeMesh = static_cast(shape.get()); + auto shapeMesh = static_cast(shape.get()); const Eigen::Vector3d& scale = shapeMesh->getScale(); auto aiScene = shapeMesh->getMesh(); @@ -619,9 +645,9 @@ boost::shared_ptr createFCLCollisionGeometry( } case Shape::SOFT_MESH: { - assert(dynamic_cast(shape.get())); + assert(dynamic_cast(shape.get())); - auto softMeshShape = static_cast(shape.get()); + auto softMeshShape = static_cast(shape.get()); auto aiMesh = softMeshShape->getAssimpMesh(); fclCollGeom.reset(createSoftMesh(aiMesh)); @@ -654,7 +680,7 @@ std::shared_ptr FCLCollisionDetector::create() FCLCollisionDetector::~FCLCollisionDetector() { assert(mShapeMap.empty()); - assert(mCollisionObjectMap.empty()); + assert(mFCLCollisionObjectMap.empty()); } //============================================================================== @@ -671,154 +697,124 @@ const std::string& FCLCollisionDetector::getType() const } //============================================================================== -FCLCollisionObjectData* FCLCollisionDetector::findCollisionObjectData( - fcl::CollisionObject* fclCollObj) const +std::shared_ptr FCLCollisionDetector::createCollisionGroup() { - auto search = mCollisionObjectMap.find(fclCollObj); - if (mCollisionObjectMap.end() != search) - return search->second; - else - return nullptr; + return std::make_shared(shared_from_this()); } //============================================================================== -CollisionObject* FCLCollisionDetector::findCollisionObject( - fcl::CollisionObject* fclCollObj) const +std::shared_ptr FCLCollisionDetector::createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) { - auto data = findCollisionObjectData(fclCollObj); - - if (data) - return data->getCollisionObject(); - else - return nullptr; + return std::make_shared(shared_from_this(), shapeFrame); } //============================================================================== -std::unique_ptr -FCLCollisionDetector::createCollisionObjectData( - CollisionObject* parent, const dynamics::ShapePtr& shape) +std::shared_ptr FCLCollisionDetector::createCollisionGroup( + const std::vector& shapeFrames) { - boost::shared_ptr fclCollGeom; - - auto findResult = mShapeMap.find(shape); - if (mShapeMap.end() != findResult) - { - fclCollGeom = findResult->second.first; - findResult->second.second++; - } - else - { - fclCollGeom = createFCLCollisionGeometry(shape); - mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); - } - - auto fclCollObjData = new FCLCollisionObjectData(this, parent, fclCollGeom); - auto fclCollObj = fclCollObjData->getFCLCollisionObject(); - - mCollisionObjectMap[fclCollObj] = fclCollObjData; - - return std::unique_ptr(fclCollObjData); + return std::make_shared(shared_from_this(), shapeFrames); } //============================================================================== -void FCLCollisionDetector::reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) +std::unique_ptr +FCLCollisionDetector::createCollisionObject(const dynamics::ShapeFrame* shapeFrame) { - // Retrieve associated shape - auto shape = collisionObjectData->getCollisionObject()->getShape(); - assert(shape); - - auto findResult = mShapeMap.find(shape); - assert(mShapeMap.end() != findResult); - - auto& fclCollGeomAndCount = findResult->second; - assert(0u != fclCollGeomAndCount.second); - - fclCollGeomAndCount.second--; + auto fclCollGeom = claimFCLCollisionGeometry(shapeFrame->getShape()); + auto collObj = new FCLCollisionObject(this, shapeFrame, fclCollGeom); + auto fclCollObj = collObj->getFCLCollisionObject(); - if (0u == fclCollGeomAndCount.second) - mShapeMap.erase(findResult); + mFCLCollisionObjectMap[fclCollObj] = collObj; - auto castedCollObjData - = static_cast(collisionObjectData); - mCollisionObjectMap.erase(castedCollObjData->getFCLCollisionObject()); + return std::unique_ptr(collObj); } //============================================================================== -std::unique_ptr -FCLCollisionDetector::createCollisionGroupData( - CollisionGroup* parent, - const CollisionDetector::CollisionObjectPtrs& collObjects) +FCLCollisionObject* FCLCollisionDetector::findCollisionObject( + fcl::CollisionObject* fclCollObj) const { - return std::unique_ptr( - new FCLCollisionGroupData(this, parent, collObjects)); + auto search = mFCLCollisionObjectMap.find(fclCollObj); + if (mFCLCollisionObjectMap.end() != search) + return search->second; + else + return nullptr; } //============================================================================== -bool FCLCollisionDetector::detect( - CollisionObjectData* objectData1, - CollisionObjectData* objectData2, - const Option& option, Result& result) +void FCLCollisionDetector::notifyDestroyingCollisionObject( + CollisionObject* collObj) { - result.contacts.clear(); + if (!collObj) + return; - assert(objectData1->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); - assert(objectData2->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); + reclaimFCLCollisionGeometry(collObj->getShape()); - auto castedData1 = static_cast(objectData1); - auto castedData2 = static_cast(objectData2); + auto casted = static_cast(collObj); + mFCLCollisionObjectMap.erase(casted->getFCLCollisionObject()); +} - auto fclCollObj1 = castedData1->getFCLCollisionObject(); - auto fclCollObj2 = castedData2->getFCLCollisionObject(); +//============================================================================== +boost::shared_ptr +FCLCollisionDetector::claimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape) +{ + boost::shared_ptr fclCollGeom; - FCLCollisionCallbackData collData(this, &option, &result); - collisionCallback(fclCollObj1, fclCollObj2, &collData); + auto search = mShapeMap.find(shape); + if (mShapeMap.end() != search) + { + auto& fclCollGeomAndCount = search->second; - return !result.contacts.empty(); + fclCollGeom = search->second.first; + + auto& count = fclCollGeomAndCount.second; + assert(0u != count); + count++; + } + else + { + fclCollGeom = createFCLCollisionGeometry(shape); + mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); + } + + return fclCollGeom; } //============================================================================== -bool FCLCollisionDetector::detect( - CollisionObjectData* objectData, - CollisionGroupData* groupData, - const Option& option, Result& result) +void FCLCollisionDetector::reclaimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape) { - result.contacts.clear(); - - assert(objectData); - assert(groupData); - assert(objectData->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); - assert(groupData->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); + auto search = mShapeMap.find(shape); + assert(mShapeMap.end() != search); - auto castedObjData = static_cast(objectData); - auto castedGrpData = static_cast(groupData); + auto& fclCollGeomAndCount = search->second; + auto& count = fclCollGeomAndCount.second; + assert(0u != count); - auto fclObject = castedObjData->getFCLCollisionObject(); - auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); + count--; - FCLCollisionCallbackData collData(this, &option, &result); - broadPhaseAlg->collide(fclObject, &collData, collisionCallback); - - return !result.contacts.empty(); + if (0u == count) + mShapeMap.erase(search); } //============================================================================== bool FCLCollisionDetector::detect( - CollisionGroupData* groupData, - const Option& option, Result& result) + CollisionGroup* group, const Option& option, Result& result) { result.contacts.clear(); - assert(groupData); - assert(groupData->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); + if (!group) + return false; - auto castedData = static_cast(groupData); + if (group->getCollisionDetector()->getType() + != FCLCollisionDetector::getTypeStatic()) + { + return false; + } + + group->update(); + auto castedData = static_cast(group); auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionCallbackData collData(this, &option, &result); @@ -829,24 +825,34 @@ bool FCLCollisionDetector::detect( //============================================================================== bool FCLCollisionDetector::detect( - CollisionGroupData* groupData1, - CollisionGroupData* groupData2, + CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { result.contacts.clear(); - assert(groupData1); - assert(groupData2); - assert(groupData1->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); - assert(groupData2->getCollisionDetector()->getType() - == FCLCollisionDetector::getTypeStatic()); + if (!group1 || !group2) + return false; + + if (group1->getCollisionDetector()->getType() + != FCLCollisionDetector::getTypeStatic()) + { + return false; + } + + if (group2->getCollisionDetector()->getType() + != FCLCollisionDetector::getTypeStatic()) + { + return false; + } + + group1->update(); + group2->update(); - auto castedData1 = static_cast(groupData1); - auto castedData2 = static_cast(groupData2); + auto casted1 = static_cast(group1); + auto casted2 = static_cast(group2); - auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); - auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); + auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); + auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); @@ -1011,9 +1017,9 @@ Contact convertContact(const fcl::Contact& fclContact, contact.triID2 = fclContact.b2; auto userData1 - = static_cast(o1->getUserData()); + = static_cast(o1->getUserData()); auto userData2 - = static_cast(o2->getUserData()); + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); contact.collisionObject1 = userData1->mCollisionObject; diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 64e89369a837e..2e3c7690077f4 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -45,7 +45,7 @@ namespace dart { namespace collision { -class FCLCollisionObjectData; +class FCLCollisionObject; class FCLCollisionDetector : public CollisionDetector { @@ -62,13 +62,18 @@ class FCLCollisionDetector : public CollisionDetector // Documentation inherited const std::string& getType() const override; - using CollisionDetector::detect; + // Documentation inherited + std::shared_ptr createCollisionGroup() override; - FCLCollisionObjectData* findCollisionObjectData( - fcl::CollisionObject* fclCollObj) const; + // Documentation inherited + std::shared_ptr createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) override; - CollisionObject* findCollisionObject( - fcl::CollisionObject* fclCollObj) const; + // Documentation inherited + std::shared_ptr createCollisionGroup( + const std::vector& shapeFrames) override; + + using CollisionDetector::detect; protected: @@ -76,33 +81,33 @@ class FCLCollisionDetector : public CollisionDetector FCLCollisionDetector() = default; // Documentation inherited - std::unique_ptr createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) override; + std::unique_ptr createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) override; - // Documentation inherited - void reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) override; +public: - // Documentation inherited - std::unique_ptr createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) override; + FCLCollisionObject* findCollisionObject( + fcl::CollisionObject* fclCollObj) const; + // TODO(JS): fix const correctness - // Documentation inherited - bool detect(CollisionObjectData* object1, CollisionObjectData* object2, - const Option& option, Result& result) override; +protected: // Documentation inherited - bool detect(CollisionObjectData* object, CollisionGroupData* group, - const Option& option, Result& result) override; + void notifyDestroyingCollisionObject(CollisionObject* collObj) override; + + /// + boost::shared_ptr claimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape); + + /// + void reclaimFCLCollisionGeometry(const dynamics::ConstShapePtr& shape); // Documentation inherited - bool detect(CollisionGroupData* group, + bool detect(CollisionGroup* group, const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) override; protected: @@ -110,9 +115,9 @@ class FCLCollisionDetector : public CollisionDetector using ShapeMapValue = std::pair, size_t>; - std::map mShapeMap; + std::map mShapeMap; - std::map mCollisionObjectMap; + std::map mFCLCollisionObjectMap; }; diff --git a/dart/collision/fcl/FCLMeshCollisionGroupData.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp similarity index 53% rename from dart/collision/fcl/FCLMeshCollisionGroupData.cpp rename to dart/collision/fcl/FCLCollisionGroup.cpp index 3c09df2948e49..0fe0ac62a0560 100644 --- a/dart/collision/fcl/FCLMeshCollisionGroupData.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -34,110 +34,120 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLMeshCollisionGroupData.h" +#include "dart/collision/fcl/FCLCollisionGroup.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLMeshCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObject.h" namespace dart { namespace collision { //============================================================================== -FCLMeshCollisionGroupData::FCLMeshCollisionGroupData( - CollisionDetector* collisionDetector, - CollisionGroup* parent, - const FCLMeshCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(collisionDetector, parent), - mBroadPhaseAlg(new FCLCollisionManager()) +FCLCollisionGroup::FCLCollisionGroup( + const CollisionDetectorPtr& collisionDetector) + : CollisionGroup(collisionDetector), + mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { - for (auto collObj : collObjects) - { - auto data = static_cast(collObj->getEngineData()); - mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); - } + assert(mCollisionDetector); +} - mBroadPhaseAlg->setup(); +//============================================================================== +FCLCollisionGroup::FCLCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame) + : CollisionGroup(collisionDetector), + mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +{ + assert(mCollisionDetector); + assert(shapeFrame); + + addShapeFrame(shapeFrame); } //============================================================================== -std::unique_ptr -FCLMeshCollisionGroupData::clone( - CollisionGroup* newParent, - const CollisionGroupData::CollisionObjectPtrs& collObjects) const +FCLCollisionGroup::FCLCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames) + : CollisionGroup(collisionDetector), + mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { - return std::unique_ptr( - new FCLMeshCollisionGroupData(mCollisionDetector, newParent, collObjects)); + assert(mCollisionDetector); + + addShapeFrames(shapeFrames); } //============================================================================== -void FCLMeshCollisionGroupData::init() +FCLCollisionGroup::~FCLCollisionGroup() +{ + removeAllShapeFrames(); +} + +//============================================================================== +void FCLCollisionGroup::initializeEngineData() { mBroadPhaseAlg->setup(); } //============================================================================== -void FCLMeshCollisionGroupData::addCollisionObject( - const CollisionObjectPtr& object, const bool init) +void FCLCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) { - auto data = static_cast( - object->getEngineData()); - mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + auto casted = static_cast(object); + mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLMeshCollisionGroupData::addCollisionObjects( - const CollisionGroupData::CollisionObjectPtrs& collObjects, const bool init) +void FCLCollisionGroup::addCollisionObjectsToEngine( + const std::vector& collObjects) { for (auto collObj : collObjects) { - auto data = static_cast( - collObj->getEngineData()); + auto casted = static_cast(collObj); - mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); } - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLMeshCollisionGroupData::removeCollisionObject( - const CollisionObjectPtr& object, const bool init) +void FCLCollisionGroup::removeCollisionObjectFromEngine(CollisionObject* object) { - auto data = static_cast( - object->getEngineData()); - mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); + auto casted = static_cast(object); - if (init) - this->init(); + mBroadPhaseAlg->unregisterObject(casted->getFCLCollisionObject()); + + this->initializeEngineData(); } //============================================================================== -void FCLMeshCollisionGroupData::removeAllCollisionObjects(bool init) +void FCLCollisionGroup::removeAllCollisionObjectsFromEngine() { mBroadPhaseAlg->clear(); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLMeshCollisionGroupData::update() +void FCLCollisionGroup::updateEngineData() { - mBroadPhaseAlg->setup(); mBroadPhaseAlg->update(); } //============================================================================== -FCLMeshCollisionGroupData::FCLCollisionManager* -FCLMeshCollisionGroupData::getFCLCollisionManager() const +FCLCollisionGroup::FCLCollisionManager* +FCLCollisionGroup::getFCLCollisionManager() { return mBroadPhaseAlg.get(); } +//============================================================================== +const FCLCollisionGroup::FCLCollisionManager* +FCLCollisionGroup::getFCLCollisionManager() const +{ + return mBroadPhaseAlg.get(); +} } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLMeshCollisionGroupData.h b/dart/collision/fcl/FCLCollisionGroup.h similarity index 64% rename from dart/collision/fcl/FCLMeshCollisionGroupData.h rename to dart/collision/fcl/FCLCollisionGroup.h index 264014c5740c3..c29a6adc42098 100644 --- a/dart/collision/fcl/FCLMeshCollisionGroupData.h +++ b/dart/collision/fcl/FCLCollisionGroup.h @@ -34,14 +34,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPDATA_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPDATA_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUP_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONGROUP_H_ #include -#include #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { @@ -49,47 +47,58 @@ namespace collision { class CollisionObject; class FCLCollisionObjectUserData; -class FCLMeshCollisionGroupData : public CollisionGroupData +class FCLCollisionGroup : public CollisionGroup { public: -// using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; - using FCLCollisionManager = fcl::NaiveCollisionManager; + friend class FCLCollisionDetector; + + using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLMeshCollisionGroupData(CollisionDetector* collisionDetector, - CollisionGroup* parent, - const CollisionObjects& collObjects); + FCLCollisionGroup( + const CollisionDetectorPtr& collisionDetector); - // Documentation inherited - std::unique_ptr clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const override; + /// Constructor + FCLCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame); + + /// Constructor + FCLCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames); + + /// Destructor + virtual ~FCLCollisionGroup(); + +protected: // Documentation inherited - void init() override; + void initializeEngineData() override; // Documentation inherited - void addCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void addCollisionObjects(const CollisionObjectPtrs& collObjects, - const bool init) override; + void addCollisionObjectsToEngine( + const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjects(bool init) override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited - void update() override; + void updateEngineData() override; + + /// Return FCL collision manager that is also a broad-phase algorithm + FCLCollisionManager* getFCLCollisionManager(); /// Return FCL collision manager that is also a broad-phase algorithm - FCLCollisionManager* getFCLCollisionManager() const; + const FCLCollisionManager* getFCLCollisionManager() const; protected: @@ -101,4 +110,4 @@ class FCLMeshCollisionGroupData : public CollisionGroupData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUP_H_ diff --git a/dart/collision/fcl/FCLCollisionObjectData.cpp b/dart/collision/fcl/FCLCollisionObject.cpp similarity index 80% rename from dart/collision/fcl/FCLCollisionObjectData.cpp rename to dart/collision/fcl/FCLCollisionObject.cpp index fb99581b59a36..dee07b9f7d426 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -34,52 +34,71 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObject.h" #include #include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" #include "dart/dynamics/SoftMeshShape.h" +#include "dart/dynamics/ShapeFrame.h" namespace dart { namespace collision { //============================================================================== -FCLCollisionObjectData::UserData::UserData(CollisionObject* collisionObject) +FCLCollisionObject::UserData::UserData(CollisionObject* collisionObject) : mCollisionObject(collisionObject) { // Do nothing } //============================================================================== -void FCLCollisionObjectData::updateTransform(const Eigen::Isometry3d& tf) +fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() { - mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); - mFCLCollisionObject->computeAABB(); + return mFCLCollisionObject.get(); +} + +//============================================================================== +const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const +{ + return mFCLCollisionObject.get(); +} + +//============================================================================== +FCLCollisionObject::FCLCollisionObject( + CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame, + const boost::shared_ptr& fclCollGeom) + : CollisionObject(collisionDetector, shapeFrame), + mFCLCollisionObjectUserData(new UserData(this)), + mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) +{ + mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); } //============================================================================== -void FCLCollisionObjectData::update() +void FCLCollisionObject::updateEngineData() { using dart::dynamics::BodyNode; using dart::dynamics::Shape; using dart::dynamics::SoftMeshShape; - auto shape = mParent->getShape().get(); + auto shape = mShapeFrame->getShape().get(); if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) { // Update soft-body's vertices if (shape->getShapeType() == Shape::SOFT_MESH) { - assert(dynamic_cast(shape)); - SoftMeshShape* softMeshShape = static_cast(shape); + assert(dynamic_cast(shape)); + auto softMeshShape = static_cast(shape); const aiMesh* mesh = softMeshShape->getAssimpMesh(); - softMeshShape->update(); + const_cast(softMeshShape)->update(); + // TODO(JS): update function be called by somewhere out of here. + #if FCL_VERSION_AT_LEAST(0,3,0) auto collGeom = const_cast( mFCLCollisionObject->collisionGeometry().get()); @@ -106,25 +125,8 @@ void FCLCollisionObjectData::update() } } - updateTransform(mParent->getTransform()); -} - -//============================================================================== -fcl::CollisionObject* FCLCollisionObjectData::getFCLCollisionObject() const -{ - return mFCLCollisionObject.get(); -} - -//============================================================================== -FCLCollisionObjectData::FCLCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent, - const boost::shared_ptr& fclCollGeom) - : CollisionObjectData(collisionDetector, parent), - mFCLCollisionObjectUserData(new UserData(parent)), - mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) -{ - mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); + mFCLCollisionObject->setTransform(FCLTypes::convertTransform(getTransform())); + mFCLCollisionObject->computeAABB(); } } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionObjectData.h b/dart/collision/fcl/FCLCollisionObject.h similarity index 76% rename from dart/collision/fcl/FCLCollisionObjectData.h rename to dart/collision/fcl/FCLCollisionObject.h index 7cea23eb26b87..57fad845f889a 100644 --- a/dart/collision/fcl/FCLCollisionObjectData.h +++ b/dart/collision/fcl/FCLCollisionObject.h @@ -34,25 +34,27 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ +#ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECT_H_ +#define DART_COLLISION_FCL_FCLCOLLISIONOBJECT_H_ #include #include #include -#include "dart/collision/CollisionObjectData.h" +#include "dart/collision/CollisionObject.h" namespace dart { namespace collision { class CollisionObject; -class FCLCollisionObjectData : public CollisionObjectData +class FCLCollisionObject : public CollisionObject { public: + friend class FCLCollisionDetector; + struct UserData { CollisionObject* mCollisionObject; @@ -60,25 +62,22 @@ class FCLCollisionObjectData : public CollisionObjectData UserData(CollisionObject* collisionObject); }; - friend class FCLCollisionDetector; - - // Documentation inherited - void updateTransform(const Eigen::Isometry3d& tf) override; - - // Documentation inherited - void update() override; + /// Return FCL collision object + fcl::CollisionObject* getFCLCollisionObject(); /// Return FCL collision object - fcl::CollisionObject* getFCLCollisionObject() const; + const fcl::CollisionObject* getFCLCollisionObject() const; protected: /// Constructor - FCLCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent, + FCLCollisionObject(CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame, const boost::shared_ptr& fclCollGeom); + // Documentation inherited + void updateEngineData() override; + protected: /// FCL collision geometry user data @@ -86,13 +85,10 @@ class FCLCollisionObjectData : public CollisionObjectData /// FCL collision object std::unique_ptr mFCLCollisionObject; - // Note: We can consider sharing fcl::CollisionGeometry with other - // CollisionObjects that are associated with the same Shape of DART to avoid - // unnecessary copy of fcl::CollisionGeometry. }; } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECTDATA_H_ +#endif // DART_COLLISION_FCL_FCLCOLLISIONOBJECT_H_ diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp index 21e016bf90b48..840a8c19a2008 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl/FCLMeshCollisionDetector.cpp @@ -49,10 +49,11 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObject.h" #include "dart/collision/fcl/tri_tri_intersection_test.h" -#include "dart/collision/fcl/FCLMeshCollisionObjectData.h" -#include "dart/collision/fcl/FCLMeshCollisionGroupData.h" +#include "dart/collision/fcl/FCLMeshCollisionObject.h" +#include "dart/collision/fcl/FCLMeshCollisionGroup.h" +#include "dart/dynamics/ShapeFrame.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" @@ -549,7 +550,7 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) //============================================================================== boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ShapePtr& shape) + const dynamics::ConstShapePtr& shape) { using ::dart::dynamics::Shape; using ::dart::dynamics::BoxShape; @@ -565,8 +566,8 @@ boost::shared_ptr createFCLCollisionGeometry( { case Shape::BOX: { - assert(dynamic_cast(shape.get())); - const BoxShape* box = static_cast(shape.get()); + assert(dynamic_cast(shape.get())); + auto box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); auto model = new fcl::BVHModel; @@ -578,8 +579,8 @@ boost::shared_ptr createFCLCollisionGeometry( } case Shape::ELLIPSOID: { - assert(dynamic_cast(shape.get())); - EllipsoidShape* ellipsoid = static_cast(shape.get()); + assert(dynamic_cast(shape.get())); + auto ellipsoid = static_cast(shape.get()); const Eigen::Vector3d& size = ellipsoid->getSize(); @@ -590,9 +591,8 @@ boost::shared_ptr createFCLCollisionGeometry( } case Shape::CYLINDER: { - assert(dynamic_cast(shape.get())); - const CylinderShape* cylinder - = static_cast(shape.get()); + assert(dynamic_cast(shape.get())); + auto cylinder = static_cast(shape.get()); const double radius = cylinder->getRadius(); const double height = cylinder->getHeight(); fclCollGeom.reset(createCylinder( @@ -608,8 +608,8 @@ boost::shared_ptr createFCLCollisionGeometry( } case Shape::MESH: { - assert(dynamic_cast(shape.get())); - MeshShape* shapeMesh = static_cast(shape.get()); + assert(dynamic_cast(shape.get())); + auto shapeMesh = static_cast(shape.get()); fclCollGeom.reset(createMesh(shapeMesh->getScale()[0], shapeMesh->getScale()[1], shapeMesh->getScale()[2], @@ -618,8 +618,8 @@ boost::shared_ptr createFCLCollisionGeometry( } case Shape::SOFT_MESH: { - assert(dynamic_cast(shape.get())); - SoftMeshShape* softMeshShape = static_cast(shape.get()); + assert(dynamic_cast(shape.get())); + auto softMeshShape = static_cast(shape.get()); fclCollGeom.reset( createSoftMesh(softMeshShape->getAssimpMesh())); @@ -652,6 +652,7 @@ std::shared_ptr FCLMeshCollisionDetector::create() FCLMeshCollisionDetector::~FCLMeshCollisionDetector() { assert(mShapeMap.empty()); + assert(mFCLCollisionObjectMap.empty()); } //============================================================================== @@ -661,158 +662,132 @@ const std::string& FCLMeshCollisionDetector::getType() const } //============================================================================== -FCLMeshCollisionObjectData* FCLMeshCollisionDetector::findCollisionObjectData( - fcl::CollisionObject* fclCollObj) const +const std::string& FCLMeshCollisionDetector::getTypeStatic() { - auto search = mCollisionObjectMap.find(fclCollObj); - if (mCollisionObjectMap.end() != search) - return search->second; - else - return nullptr; + static const std::string& type("FCLMesh"); + return type; } //============================================================================== -CollisionObject* FCLMeshCollisionDetector::findCollisionObject( - fcl::CollisionObject* fclCollObj) const +std::shared_ptr FCLMeshCollisionDetector::createCollisionGroup() { - auto data = findCollisionObjectData(fclCollObj); - - if (data) - return data->getCollisionObject(); - else - return nullptr; + return std::make_shared(shared_from_this()); } //============================================================================== -const std::string& FCLMeshCollisionDetector::getTypeStatic() +std::shared_ptr FCLMeshCollisionDetector::createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) { - static const std::string& type("FCLMesh"); - return type; + return std::make_shared(shared_from_this(), + shapeFrame); } //============================================================================== -std::unique_ptr -FCLMeshCollisionDetector::createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) +std::shared_ptr FCLMeshCollisionDetector::createCollisionGroup( + const std::vector& shapeFrames) { - boost::shared_ptr fclCollGeom; - - auto findResult = mShapeMap.find(shape); - if (mShapeMap.end() != findResult) - { - fclCollGeom = findResult->second.first; - findResult->second.second++; - } - else - { - fclCollGeom = createFCLCollisionGeometry(shape); - mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); - } - - auto fclCollObjData - = new FCLMeshCollisionObjectData(this, parent, fclCollGeom); - auto fclCollObj = fclCollObjData->getFCLCollisionObject(); - - mCollisionObjectMap[fclCollObj] = fclCollObjData; - - return std::unique_ptr(fclCollObjData); + return std::make_shared(shared_from_this(), + shapeFrames); } //============================================================================== -void FCLMeshCollisionDetector::reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) +std::unique_ptr +FCLMeshCollisionDetector::createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { - // Retrieve associated shape - auto shape = collisionObjectData->getCollisionObject()->getShape(); - assert(shape); - - auto findResult = mShapeMap.find(shape); - assert(mShapeMap.end() != findResult); + auto fclCollGeom = claimFCLCollisionGeometry(shapeFrame->getShape()); + auto collObj = new FCLMeshCollisionObject(this, shapeFrame, fclCollGeom); + auto fclCollObj = collObj->getFCLCollisionObject(); - auto& fclCollGeomAndCount = findResult->second; - assert(0u != fclCollGeomAndCount.second); + mFCLCollisionObjectMap[fclCollObj] = collObj; - fclCollGeomAndCount.second--; - - if (0u == fclCollGeomAndCount.second) - mShapeMap.erase(findResult); - - auto castedCollObjData - = static_cast(collisionObjectData); - mCollisionObjectMap.erase(castedCollObjData->getFCLCollisionObject()); + return std::unique_ptr(collObj); } //============================================================================== -std::unique_ptr -FCLMeshCollisionDetector::createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) +FCLMeshCollisionObject* FCLMeshCollisionDetector::findCollisionObject( + fcl::CollisionObject* fclCollObj) const { - return std::unique_ptr( - new FCLMeshCollisionGroupData(this, parent, collObjects)); + auto search = mFCLCollisionObjectMap.find(fclCollObj); + if (mFCLCollisionObjectMap.end() != search) + return search->second; + else + return nullptr; } //============================================================================== -bool FCLMeshCollisionDetector::detect( - CollisionObjectData* objectData1, - CollisionObjectData* objectData2, - const Option& option, Result& result) +void FCLMeshCollisionDetector::notifyDestroyingCollisionObject( + CollisionObject* collObj) { - result.contacts.clear(); + if (!collObj) + return; - assert(objectData1->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); - assert(objectData2->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + reclaimFCLCollisionGeometry(collObj->getShape()); - auto castedData1 = static_cast(objectData1); - auto castedData2 = static_cast(objectData2); + auto casted = static_cast(collObj); + mFCLCollisionObjectMap.erase(casted->getFCLCollisionObject()); +} - auto fclCollObj1 = castedData1->getFCLCollisionObject(); - auto fclCollObj2 = castedData2->getFCLCollisionObject(); +//============================================================================== +boost::shared_ptr +FCLMeshCollisionDetector::claimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape) +{ + boost::shared_ptr fclCollGeom; - FCLCollisionCallbackData collData(this, &option, &result); - collisionCallback(fclCollObj1, fclCollObj2, &collData); + auto search = mShapeMap.find(shape); + if (mShapeMap.end() != search) + { + auto& fclCollGeomAndCount = search->second; - return !result.contacts.empty(); + fclCollGeom = search->second.first; + + auto& count = fclCollGeomAndCount.second; + assert(0u != count); + count++; + } + else + { + fclCollGeom = createFCLCollisionGeometry(shape); + mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); + } + + return fclCollGeom; } //============================================================================== -bool FCLMeshCollisionDetector::detect( - CollisionObjectData* objectData, - CollisionGroupData* groupData, - const Option& option, Result& result) +void FCLMeshCollisionDetector::reclaimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape) { - result.contacts.clear(); + auto search = mShapeMap.find(shape); + assert(mShapeMap.end() != search); - assert(objectData); - assert(groupData); - assert(objectData->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); - assert(groupData->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + auto& fclCollGeomAndCount = search->second; + auto& count = fclCollGeomAndCount.second; + assert(0u != count); - auto castedObjData = static_cast(objectData); - auto castedGrpData = static_cast(groupData); + count--; - auto fclObject = castedObjData->getFCLCollisionObject(); - auto broadPhaseAlg = castedGrpData->getFCLCollisionManager(); - - FCLCollisionCallbackData collData(this, &option, &result); - broadPhaseAlg->collide(fclObject, &collData, collisionCallback); - - return !result.contacts.empty(); + if (0u == count) + mShapeMap.erase(search); } //============================================================================== bool FCLMeshCollisionDetector::detect( - CollisionGroupData* groupData, - const Option& option, Result& result) + CollisionGroup* group, const Option& option, Result& result) { result.contacts.clear(); - assert(groupData); - assert(groupData->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + if (!group) + return false; - auto castedData = static_cast(groupData); + if (group->getCollisionDetector()->getType() + != FCLMeshCollisionDetector::getTypeStatic()) + { + return false; + } + auto castedData = static_cast(group); auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionCallbackData collData(this, &option, &result); @@ -823,22 +798,34 @@ bool FCLMeshCollisionDetector::detect( //============================================================================== bool FCLMeshCollisionDetector::detect( - CollisionGroupData* groupData1, - CollisionGroupData* groupData2, + CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { result.contacts.clear(); - assert(groupData1); - assert(groupData2); - assert(groupData1->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); - assert(groupData2->getCollisionDetector()->getType() == FCLMeshCollisionDetector::getTypeStatic()); + if (!group1 || !group2) + return false; + + if (group1->getCollisionDetector()->getType() + != FCLMeshCollisionDetector::getTypeStatic()) + { + return false; + } + + if (group2->getCollisionDetector()->getType() + != FCLMeshCollisionDetector::getTypeStatic()) + { + return false; + } + + group1->update(); + group2->update(); - auto castedData1 = static_cast(groupData1); - auto castedData2 = static_cast(groupData2); + auto casted1 = static_cast(group1); + auto casted2 = static_cast(group2); - auto broadPhaseAlg1 = castedData1->getFCLCollisionManager(); - auto broadPhaseAlg2 = castedData2->getFCLCollisionManager(); + auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); + auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); FCLCollisionCallbackData collData(this, &option, &result); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); @@ -926,9 +913,9 @@ void postProcess(const fcl::CollisionResult& fclResult, const auto& c = fclResult.getContact(k); auto userData1 - = static_cast(o1->getUserData()); + = static_cast(o1->getUserData()); auto userData2 - = static_cast(o2->getUserData()); + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); @@ -1169,9 +1156,9 @@ Contact convertContact(const fcl::Contact& fclContact, contact.triID2 = fclContact.b2; auto userData1 - = static_cast(o1->getUserData()); + = static_cast(o1->getUserData()); auto userData2 - = static_cast(o2->getUserData()); + = static_cast(o2->getUserData()); assert(userData1); assert(userData2); contact.collisionObject1 = userData1->mCollisionObject; diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.h b/dart/collision/fcl/FCLMeshCollisionDetector.h index 1fce89501b61a..f6a9ff0480943 100644 --- a/dart/collision/fcl/FCLMeshCollisionDetector.h +++ b/dart/collision/fcl/FCLMeshCollisionDetector.h @@ -44,7 +44,7 @@ namespace dart { namespace collision { -class FCLMeshCollisionObjectData; +class FCLMeshCollisionObject; class FCLMeshCollisionDetector : public CollisionDetector { @@ -61,13 +61,18 @@ class FCLMeshCollisionDetector : public CollisionDetector // Documentation inherited const std::string& getType() const override; - using CollisionDetector::detect; + // Documentation inherited + std::shared_ptr createCollisionGroup() override; - FCLMeshCollisionObjectData* findCollisionObjectData( - fcl::CollisionObject* fclCollObj) const; + // Documentation inherited + std::shared_ptr createCollisionGroup( + const dynamics::ShapeFrame* shapeFrame) override; - CollisionObject* findCollisionObject( - fcl::CollisionObject* fclCollObj) const; + // Documentation inherited + std::shared_ptr createCollisionGroup( + const std::vector& shapeFrames) override; + + using CollisionDetector::detect; protected: @@ -75,33 +80,31 @@ class FCLMeshCollisionDetector : public CollisionDetector FCLMeshCollisionDetector() = default; // Documentation inherited - std::unique_ptr createCollisionObjectData( - CollisionObject* parent, - const dynamics::ShapePtr& shape) override; + std::unique_ptr createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) override; - // Documentation inherited - void reclaimCollisionObjectData( - CollisionObjectData* collisionObjectData) override; +public: - // Documentation inherited - std::unique_ptr createCollisionGroupData( - CollisionGroup* parent, - const CollisionObjectPtrs& collObjects) override; + FCLMeshCollisionObject* findCollisionObject( + fcl::CollisionObject* fclCollObj) const; + +protected: // Documentation inherited - bool detect(CollisionObjectData* object1, CollisionObjectData* object2, - const Option& option, Result& result) override; + void notifyDestroyingCollisionObject(CollisionObject* collObj) override; + + boost::shared_ptr claimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape); // Documentation inherited - bool detect(CollisionObjectData* object, CollisionGroupData* group, - const Option& option, Result& result) override; + void reclaimFCLCollisionGeometry(const dynamics::ConstShapePtr& shape); // Documentation inherited - bool detect(CollisionGroupData* group, + bool detect(CollisionGroup* group, const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionGroupData* group1, CollisionGroupData* group2, + bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) override; protected: @@ -109,10 +112,10 @@ class FCLMeshCollisionDetector : public CollisionDetector using ShapeMapValue = std::pair, size_t>; - std::map mShapeMap; + std::map mShapeMap; std::map mCollisionObjectMap; + FCLMeshCollisionObject*> mFCLCollisionObjectMap; }; diff --git a/dart/collision/fcl/FCLCollisionGroupData.cpp b/dart/collision/fcl/FCLMeshCollisionGroup.cpp similarity index 56% rename from dart/collision/fcl/FCLCollisionGroupData.cpp rename to dart/collision/fcl/FCLMeshCollisionGroup.cpp index 5f53a00c0572d..c05ba5b5a2b6e 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.cpp +++ b/dart/collision/fcl/FCLMeshCollisionGroup.cpp @@ -34,106 +34,109 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLCollisionGroupData.h" +#include "dart/collision/fcl/FCLMeshCollisionGroup.h" #include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLMeshCollisionObject.h" namespace dart { namespace collision { //============================================================================== -FCLCollisionGroupData::FCLCollisionGroupData( - CollisionDetector* collisionDetector, - CollisionGroup* parent, - const FCLCollisionGroupData::CollisionObjects& collObjects) - : CollisionGroupData(collisionDetector, parent), +FCLMeshCollisionGroup::FCLMeshCollisionGroup( + const CollisionDetectorPtr& collisionDetector) + : CollisionGroup(collisionDetector), mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { - for (auto collObj : collObjects) - { - auto data = static_cast(collObj->getEngineData()); - mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); - } + assert(mCollisionDetector); +} - mBroadPhaseAlg->setup(); +//============================================================================== +FCLMeshCollisionGroup::FCLMeshCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame) + : CollisionGroup(collisionDetector), + mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +{ + assert(mCollisionDetector); + assert(shapeFrame); + + addShapeFrame(shapeFrame); +} + +//============================================================================== +FCLMeshCollisionGroup::FCLMeshCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames) + : CollisionGroup(collisionDetector), + mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) +{ + assert(mCollisionDetector); + + addShapeFrames(shapeFrames); } //============================================================================== -std::unique_ptr -FCLCollisionGroupData::clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const +FCLMeshCollisionGroup::~FCLMeshCollisionGroup() { - return std::unique_ptr( - new FCLCollisionGroupData(mCollisionDetector, newParent, collObjects)); + removeAllShapeFrames(); } //============================================================================== -void FCLCollisionGroupData::init() +void FCLMeshCollisionGroup::initializeEngineData() { mBroadPhaseAlg->setup(); } //============================================================================== -void FCLCollisionGroupData::addCollisionObject( - const CollisionObjectPtr& object, const bool init) +void FCLMeshCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) { - auto data = static_cast( - object->getEngineData()); - mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + auto casted = static_cast(object); + mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLCollisionGroupData::addCollisionObjects( - const FCLCollisionGroupData::CollisionObjectPtrs& collObjects, - const bool init) +void FCLMeshCollisionGroup::addCollisionObjectsToEngine( + const std::vector& collObjects) { for (auto collObj : collObjects) { - auto data = static_cast( - collObj->getEngineData()); + auto casted = static_cast(collObj); - mBroadPhaseAlg->registerObject(data->getFCLCollisionObject()); + mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); } - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLCollisionGroupData::removeCollisionObject( - const CollisionObjectPtr& object, const bool init) +void FCLMeshCollisionGroup::removeCollisionObjectFromEngine(CollisionObject* object) { - auto data = static_cast( - object->getEngineData()); - mBroadPhaseAlg->unregisterObject(data->getFCLCollisionObject()); + auto casted = static_cast(object); + mBroadPhaseAlg->unregisterObject(casted->getFCLCollisionObject()); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLCollisionGroupData::removeAllCollisionObjects(bool init) +void FCLMeshCollisionGroup::removeAllCollisionObjectsFromEngine() { mBroadPhaseAlg->clear(); - if (init) - this->init(); + this->initializeEngineData(); } //============================================================================== -void FCLCollisionGroupData::update() +void FCLMeshCollisionGroup::updateEngineData() { mBroadPhaseAlg->update(); } //============================================================================== -FCLCollisionGroupData::FCLCollisionManager* -FCLCollisionGroupData::getFCLCollisionManager() const +FCLMeshCollisionGroup::FCLCollisionManager* +FCLMeshCollisionGroup::getFCLCollisionManager() const { return mBroadPhaseAlg.get(); } diff --git a/dart/collision/fcl/FCLCollisionGroupData.h b/dart/collision/fcl/FCLMeshCollisionGroup.h similarity index 69% rename from dart/collision/fcl/FCLCollisionGroupData.h rename to dart/collision/fcl/FCLMeshCollisionGroup.h index 4b303c682378e..e41b6a411cc3e 100644 --- a/dart/collision/fcl/FCLCollisionGroupData.h +++ b/dart/collision/fcl/FCLMeshCollisionGroup.h @@ -34,13 +34,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ -#define DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONGROUP_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONGROUP_H_ #include +#include #include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionGroupData.h" namespace dart { namespace collision { @@ -48,43 +48,52 @@ namespace collision { class CollisionObject; class FCLCollisionObjectUserData; -class FCLCollisionGroupData : public CollisionGroupData +class FCLMeshCollisionGroup : public CollisionGroup { public: + friend class FCLMeshCollisionDetector; + using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor - FCLCollisionGroupData(CollisionDetector* collisionDetector, - CollisionGroup* parent, - const CollisionObjects& collObjects); + FCLMeshCollisionGroup( + const CollisionDetectorPtr& collisionDetector); - // Documentation inherited - std::unique_ptr clone( - CollisionGroup* newParent, - const CollisionObjectPtrs& collObjects) const override; + /// Constructor + FCLMeshCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const dynamics::ShapeFrame* shapeFrame); + + /// Constructor + FCLMeshCollisionGroup( + const CollisionDetectorPtr& collisionDetector, + const std::vector& shapeFrames); + + /// Destructor + virtual ~FCLMeshCollisionGroup(); + +protected: // Documentation inherited - void init() override; + void initializeEngineData() override; // Documentation inherited - void addCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void addCollisionObjects(const CollisionObjectPtrs& collObjects, - const bool init) override; + void addCollisionObjectsToEngine( + const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObject(const CollisionObjectPtr& object, - const bool init) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjects(bool init) override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited - void update() override; + void updateEngineData() override; /// Return FCL collision manager that is also a broad-phase algorithm FCLCollisionManager* getFCLCollisionManager() const; @@ -99,4 +108,4 @@ class FCLCollisionGroupData : public CollisionGroupData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLCOLLISIONGROUPDATA_H_ +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONGROUP_H_ diff --git a/dart/collision/fcl/FCLMeshCollisionObjectData.cpp b/dart/collision/fcl/FCLMeshCollisionObject.cpp similarity index 80% rename from dart/collision/fcl/FCLMeshCollisionObjectData.cpp rename to dart/collision/fcl/FCLMeshCollisionObject.cpp index 5297066be0db4..721663d73618d 100644 --- a/dart/collision/fcl/FCLMeshCollisionObjectData.cpp +++ b/dart/collision/fcl/FCLMeshCollisionObject.cpp @@ -34,29 +34,46 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/fcl/FCLMeshCollisionObjectData.h" +#include "dart/collision/fcl/FCLMeshCollisionObject.h" #include #include "dart/collision/CollisionDetector.h" #include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/fcl/FCLCollisionObject.h" +#include "dart/dynamics/ShapeFrame.h" #include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { //============================================================================== -void FCLMeshCollisionObjectData::updateTransform( - const Eigen::Isometry3d& tf) +fcl::CollisionObject* FCLMeshCollisionObject::getFCLCollisionObject() { - mFCLCollisionObject->setTransform(FCLTypes::convertTransform(tf)); - mFCLCollisionObject->computeAABB(); + return mFCLCollisionObject.get(); +} + +//============================================================================== +const fcl::CollisionObject* FCLMeshCollisionObject::getFCLCollisionObject() const +{ + return mFCLCollisionObject.get(); +} + +//============================================================================== +FCLMeshCollisionObject::FCLMeshCollisionObject( + CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame, + const boost::shared_ptr& fclCollGeom) + : CollisionObject(collisionDetector, shapeFrame), + mFCLCollisionObjectUserData(new FCLCollisionObject::UserData(this)), + mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) +{ + mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); } //============================================================================== -void FCLMeshCollisionObjectData::update() +void FCLMeshCollisionObject::updateEngineData() { using dart::dynamics::BodyNode; using dart::dynamics::Shape; @@ -70,10 +87,11 @@ void FCLMeshCollisionObjectData::update() // Update soft-body's vertices if (shape->getShapeType() == Shape::SOFT_MESH) { - assert(dynamic_cast(shape)); - SoftMeshShape* softMeshShape = static_cast(shape); + assert(dynamic_cast(shape)); + auto softMeshShape = static_cast(shape); const aiMesh* mesh = softMeshShape->getAssimpMesh(); - softMeshShape->update(); + const_cast(softMeshShape)->update(); + // TODO(JS): update function be called by somewhere out of here. #if FCL_VERSION_AT_LEAST(0,3,0) auto collGeom = const_cast( @@ -103,25 +121,9 @@ void FCLMeshCollisionObjectData::update() } } - updateTransform(collisionObject->getTransform()); -} - -//============================================================================== -fcl::CollisionObject* FCLMeshCollisionObjectData::getFCLCollisionObject() const -{ - return mFCLCollisionObject.get(); -} - -//============================================================================== -FCLMeshCollisionObjectData::FCLMeshCollisionObjectData( - CollisionDetector* collisionDetector, - CollisionObject* parent, - const boost::shared_ptr& fclCollGeom) - : CollisionObjectData(collisionDetector, parent), - mFCLCollisionObjectUserData(new FCLCollisionObjectData::UserData(parent)), - mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) -{ - mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); + mFCLCollisionObject->setTransform( + FCLTypes::convertTransform(mShapeFrame->getWorldTransform())); + mFCLCollisionObject->computeAABB(); } } // namespace collision diff --git a/dart/collision/fcl/FCLMeshCollisionObjectData.h b/dart/collision/fcl/FCLMeshCollisionObject.h similarity index 77% rename from dart/collision/fcl/FCLMeshCollisionObjectData.h rename to dart/collision/fcl/FCLMeshCollisionObject.h index c582d4540a59d..40380fbe0b991 100644 --- a/dart/collision/fcl/FCLMeshCollisionObjectData.h +++ b/dart/collision/fcl/FCLMeshCollisionObject.h @@ -34,47 +34,47 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTDATA_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTDATA_H_ +#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECT_H_ +#define DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECT_H_ #include #include #include -#include "dart/collision/CollisionObjectData.h" -#include "dart/collision/fcl/FCLCollisionObjectData.h" +#include "dart/collision/CollisionObject.h" +#include "dart/collision/fcl/FCLCollisionObject.h" namespace dart { namespace collision { -class FCLMeshCollisionObjectData : public CollisionObjectData +class FCLMeshCollisionObject : public CollisionObject { public: friend class FCLMeshCollisionDetector; - // Documentation inherited - void updateTransform(const Eigen::Isometry3d& tf) override; - - // Documentation inherited - void update() override; + /// Return FCL collision object + fcl::CollisionObject* getFCLCollisionObject(); /// Return FCL collision object - fcl::CollisionObject* getFCLCollisionObject() const; + const fcl::CollisionObject* getFCLCollisionObject() const; protected: /// Constructor - FCLMeshCollisionObjectData( + FCLMeshCollisionObject( CollisionDetector* collisionDetector, - CollisionObject* parent, + const dynamics::ShapeFrame* shapeFrame, const boost::shared_ptr& fclCollGeom); + // Documentation inherited + void updateEngineData() override; + protected: /// FCL collision geometry user data - std::unique_ptr mFCLCollisionObjectUserData; + std::unique_ptr mFCLCollisionObjectUserData; /// FCL collision object std::unique_ptr mFCLCollisionObject; @@ -84,4 +84,4 @@ class FCLMeshCollisionObjectData : public CollisionObjectData } // namespace collision } // namespace dart -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECTDATA_H_ +#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECT_H_ diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 163ae0e07e112..a7b1a92ffd2d2 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -90,7 +90,7 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) if (!containSkeleton(skeleton)) { - auto group = createShapeFrameCollisionGroup(mCollisionDetector, skeleton); + auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); mCollisionGroup->unionGroup(group); mSkeletons.push_back(skeleton); @@ -119,7 +119,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) if (containSkeleton(skeleton)) { - auto group = createShapeFrameCollisionGroup(mCollisionDetector, skeleton); + auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); mCollisionGroup->subtractGroup(group); mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), @@ -145,7 +145,7 @@ void ConstraintSolver::removeSkeletons( //============================================================================== void ConstraintSolver::removeAllSkeletons() { - mCollisionGroup->removeAllCollisionObjects(); + mCollisionGroup->removeAllShapeFrames(); mSkeletons.clear(); } @@ -220,8 +220,11 @@ void ConstraintSolver::setCollisionDetector( // Change the collision detector of the constraint solver to new one mCollisionDetector = collisionDetector; - auto newCollisionGroup = mCollisionDetector->createCollisionGroup( - mCollisionGroup->getCollisionObjects()); + auto newCollisionGroup = mCollisionDetector->createCollisionGroup(); + + for (const auto& skeleton : mSkeletons) + newCollisionGroup->addShapeFrames(skeleton.get()); + mCollisionGroup = newCollisionGroup; } @@ -543,26 +546,21 @@ void ConstraintSolver::solveConstrainedGroups() } //============================================================================== -bool ConstraintSolver::isSoftContact(const collision::Contact& _contact) const +bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const { - assert(dynamic_cast( - _contact.collisionObject1)); - assert(dynamic_cast( - _contact.collisionObject2)); - - auto shapeFrameCollObj1 = static_cast( - _contact.collisionObject1); - auto shapeFrameCollObj2 = static_cast( - _contact.collisionObject2); + auto shapeNode1 = contact.collisionObject1->getShapeFrame()->asShapeNode(); + auto shapeNode2 = contact.collisionObject2->getShapeFrame()->asShapeNode(); + assert(shapeNode1); + assert(shapeNode2); - auto bodyNode1 = shapeFrameCollObj1->getBodyNode(); - auto bodyNode2 = shapeFrameCollObj2->getBodyNode(); + auto bodyNode1 = shapeNode1->getBodyNodePtr().get(); + auto bodyNode2 = shapeNode2->getBodyNodePtr().get(); auto bodyNode1IsSoft = - dynamic_cast(bodyNode1) != nullptr; + dynamic_cast(bodyNode1) != nullptr; auto bodyNode2IsSoft = - dynamic_cast(bodyNode2) != nullptr; + dynamic_cast(bodyNode2) != nullptr; return bodyNode1IsSoft || bodyNode2IsSoft; } diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 42a6ee6d5a546..bf29588e56719 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -70,20 +70,20 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, double _timeStep) : ConstraintBase(), mTimeStep(_timeStep), - mBodyNode1(static_cast( - _contact.collisionObject1)->getBodyNode()), - mBodyNode2(static_cast( - _contact.collisionObject2)->getBodyNode()), +// mBodyNode1(static_cast( +// _contact.collisionObject1)->getBodyNode()), +// mBodyNode2(static_cast( +// _contact.collisionObject2)->getBodyNode()), mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), mIsFrictionOn(true), mAppliedImpulseIndex(-1), mIsBounceOn(false), mActive(false) { - assert(dynamic_cast( - _contact.collisionObject1)->getBodyNode()); - assert(dynamic_cast( - _contact.collisionObject2)->getBodyNode()); +// assert(dynamic_cast( +// _contact.collisionObject1)->getBodyNode()); +// assert(dynamic_cast( +// _contact.collisionObject2)->getBodyNode()); // TODO(JS): Assumed single contact mContacts.push_back(&_contact); diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index 99ae28eea792e..2c9d0f3d8a7fe 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -70,31 +70,24 @@ double SoftContactConstraint::mConstraintForceMixing = DART_CFM; //============================================================================== SoftContactConstraint::SoftContactConstraint( - collision::Contact& _contact, double _timeStep) + collision::Contact& contact, double timeStep) : ConstraintBase(), - mTimeStep(_timeStep), - mBodyNode1(static_cast( - _contact.collisionObject1)->getBodyNode()), - mBodyNode2(static_cast( - _contact.collisionObject2)->getBodyNode()), + mTimeStep(timeStep), + mBodyNode1(const_cast(contact.collisionObject1->getShapeFrame())->asShapeNode()->getBodyNodePtr().get()), + mBodyNode2(const_cast(contact.collisionObject2->getShapeFrame())->asShapeNode()->getBodyNodePtr().get()), mSoftBodyNode1(dynamic_cast(mBodyNode1)), mSoftBodyNode2(dynamic_cast(mBodyNode2)), mPointMass1(nullptr), mPointMass2(nullptr), - mSoftCollInfo(static_cast(_contact.userData)), + mSoftCollInfo(static_cast(contact.userData)), mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), mIsFrictionOn(true), mAppliedImpulseIndex(-1), mIsBounceOn(false), mActive(false) { - assert(dynamic_cast( - _contact.collisionObject1)->getBodyNode()); - assert(dynamic_cast( - _contact.collisionObject2)->getBodyNode()); - // TODO(JS): Assumed single contact - mContacts.push_back(&_contact); + mContacts.push_back(&contact); // Set the colliding state of body nodes and point masses to false if (mSoftBodyNode1) @@ -111,20 +104,20 @@ SoftContactConstraint::SoftContactConstraint( // Select colling point mass based on trimesh ID if (mSoftBodyNode1) { - if (_contact.collisionObject1->getShape()->getShapeType() + if (contact.collisionObject1->getShape()->getShapeType() == dynamics::Shape::SOFT_MESH) { - mPointMass1 = selectCollidingPointMass(mSoftBodyNode1, _contact.point, - _contact.triID1); + mPointMass1 = selectCollidingPointMass(mSoftBodyNode1, contact.point, + contact.triID1); } } if (mSoftBodyNode2) { - if (_contact.collisionObject2->getShape()->getShapeType() + if (contact.collisionObject2->getShape()->getShapeType() == dynamics::Shape::SOFT_MESH) { - mPointMass2 = selectCollidingPointMass(mSoftBodyNode2, _contact.point, - _contact.triID2); + mPointMass2 = selectCollidingPointMass(mSoftBodyNode2, contact.point, + contact.triID2); } } diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index a140ebdc5a487..673c3a0833cb4 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -49,11 +49,15 @@ bool BodyNodeCollisionFilter::needCollision( if (object1 == object2) return false; - auto castedObj1 = static_cast(object1); - auto castedObj2 = static_cast(object2); + auto shapeNode1 = object1->getShapeFrame()->asShapeNode(); + auto shapeNode2 = object2->getShapeFrame()->asShapeNode(); - auto bodyNode1 = castedObj1->getBodyNode(); - auto bodyNode2 = castedObj2->getBodyNode(); + if (!shapeNode1 || !shapeNode2) + return true; + // Assume non-ShapeNode always collides with others. + + auto bodyNode1 = shapeNode1->getBodyNodePtr(); + auto bodyNode2 = shapeNode2->getBodyNodePtr(); if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable()) return false; @@ -89,129 +93,129 @@ bool BodyNodeCollisionFilter::isAdjacentBodies(const BodyNode* bodyNode1, return false; } -//============================================================================== -const Eigen::Isometry3d ShapeNodeCollisionObject::getTransform() const -{ - return mShapeNode->getWorldTransform(); -} - -//============================================================================== -bool ShapeNodeCollisionObject::isEqual( - const collision::CollisionObject* other) const -{ - if (this == other) - return true; - - auto castedOther = dynamic_cast(other); - - if (!castedOther) - return false; - - if (mShape != castedOther->mShape) - return false; - - if (mShapeNode != castedOther->mShapeNode) - return false; - - // If castedOther has the same shape and body node then it must be the same - // pointer with this as well because the collision detector doesn't allow to - // create new collision objects for the same shape and body node. - assert(this != other); - - return true; -} - -//============================================================================== -ShapeNode* ShapeNodeCollisionObject::getShapeNode() -{ - return mShapeNode.get(); -} - -//============================================================================== -const ShapeNode* ShapeNodeCollisionObject::getShapeNode() const -{ - return mShapeNode.get(); -} - -//============================================================================== -BodyNode* ShapeNodeCollisionObject::getBodyNode() -{ - return mShapeNode->getBodyNodePtr().get(); -} - -//============================================================================== -const BodyNode* ShapeNodeCollisionObject::getBodyNode() const -{ - return mShapeNode->getBodyNodePtr().get(); -} - -//============================================================================== -ShapeNodeCollisionObject::ShapeNodeCollisionObject( - const collision::CollisionDetectorPtr& collisionDetector, - const ShapePtr& shape, - const ShapeNodePtr& shapeNode) - : collision::CollisionObject(collisionDetector, shape), - mShapeNode(shapeNode) -{ - assert(collisionDetector); - assert(shapeNode); - assert(shapeNode->getShape()); - assert(shape == shapeNode->getShape()); - - if (!mShapeNode->hasCollisionAddon()) - { - dtwarn << "[ShapeFrameCollisionObject::constructor] Attempting to create " - << "ShapeFrameCollisionObject with invalid pair of Shape and " - << "BodyNode.\n"; - assert(false); - } -} - -//============================================================================== -ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( - const collision::CollisionDetectorPtr& collisionDetector, - const ShapeNodePtr& shapeNode) -{ - return collisionDetector->createCollisionObject( - shapeNode->getShape(), shapeNode); -} - -//============================================================================== -std::vector createShapeFrameCollisionObjects( - const collision::CollisionDetectorPtr& collisionDetector, - const SkeletonPtr& skel) -{ - std::vector objects; - - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - { - auto bodyNode = skel->getBodyNode(i); - auto collisionShapeNodes = bodyNode->getShapeNodesWith(); - - for (auto& shapeNode : collisionShapeNodes) - { - auto collObj - = createShapeNodeCollisionObject(collisionDetector, shapeNode); - - objects.push_back( - std::static_pointer_cast(collObj)); - } - } - - return objects; -} - -//============================================================================== -collision::CollisionGroupPtr createShapeFrameCollisionGroup( - const collision::CollisionDetectorPtr& collisionDetector, - const SkeletonPtr& skel) -{ - auto collObjs = createShapeFrameCollisionObjects(collisionDetector, skel); - auto group = collisionDetector->createCollisionGroup(collObjs); - - return group; -} +////============================================================================== +//const Eigen::Isometry3d ShapeNodeCollisionObject::getTransform() const +//{ +// return mShapeNode->getWorldTransform(); +//} + +////============================================================================== +//bool ShapeNodeCollisionObject::isEqual( +// const collision::CollisionObject* other) const +//{ +// if (this == other) +// return true; + +// auto castedOther = dynamic_cast(other); + +// if (!castedOther) +// return false; + +// if (mShapeFrame != castedOther->mShapeFrame) +// return false; + +// if (mShapeNode != castedOther->mShapeNode) +// return false; + +// // If castedOther has the same shape and body node then it must be the same +// // pointer with this as well because the collision detector doesn't allow to +// // create new collision objects for the same shape and body node. +// assert(this != other); + +// return true; +//} + +////============================================================================== +//ShapeNode* ShapeNodeCollisionObject::getShapeNode() +//{ +// return mShapeNode.get(); +//} + +////============================================================================== +//const ShapeNode* ShapeNodeCollisionObject::getShapeNode() const +//{ +// return mShapeNode.get(); +//} + +////============================================================================== +//BodyNode* ShapeNodeCollisionObject::getBodyNode() +//{ +// return mShapeNode->getBodyNodePtr().get(); +//} + +////============================================================================== +//const BodyNode* ShapeNodeCollisionObject::getBodyNode() const +//{ +// return mShapeNode->getBodyNodePtr().get(); +//} + +////============================================================================== +//ShapeNodeCollisionObject::ShapeNodeCollisionObject( +// const collision::CollisionDetectorPtr& collisionDetector, +// const ShapePtr& shape, +// const ShapeNodePtr& shapeNode) +// : collision::CollisionObject(collisionDetector, shape), +// mShapeNode(shapeNode) +//{ +// assert(collisionDetector); +// assert(shapeNode); +// assert(shapeNode->getShape()); +// assert(shape == shapeNode->getShape()); + +// if (!mShapeNode->hasCollisionAddon()) +// { +// dtwarn << "[ShapeFrameCollisionObject::constructor] Attempting to create " +// << "ShapeFrameCollisionObject with invalid pair of Shape and " +// << "BodyNode.\n"; +// assert(false); +// } +//} + +////============================================================================== +//ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( +// const collision::CollisionDetectorPtr& collisionDetector, +// const ShapeNodePtr& shapeNode) +//{ +// return collisionDetector->getOrCreateCollisionObject( +// shapeNode->getShape(), shapeNode); +//} + +////============================================================================== +//std::vector createShapeFrameCollisionObjects( +// const collision::CollisionDetectorPtr& collisionDetector, +// const SkeletonPtr& skel) +//{ +// std::vector objects; + +// auto numBodyNodes = skel->getNumBodyNodes(); +// for (auto i = 0u; i < numBodyNodes; ++i) +// { +// auto bodyNode = skel->getBodyNode(i); +// auto collisionShapeNodes = bodyNode->getShapeNodesWith(); + +// for (auto& shapeNode : collisionShapeNodes) +// { +// auto collObj +// = createShapeNodeCollisionObject(collisionDetector, shapeNode); + +// objects.push_back( +// std::static_pointer_cast(collObj)); +// } +// } + +// return objects; +//} + +////============================================================================== +//collision::CollisionGroupPtr createShapeFrameCollisionGroup( +// const collision::CollisionDetectorPtr& collisionDetector, +// const SkeletonPtr& skel) +//{ +// auto collObjs = createShapeFrameCollisionObjects(collisionDetector, skel); +// auto group = collisionDetector->createCollisionGroup(collObjs); + +// return group; +//} } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index b0a5828b8f6fd..0c0efa50c597a 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -57,57 +57,57 @@ struct BodyNodeCollisionFilter : collision::CollisionFilter const BodyNode* bodyNode2) const; }; -class ShapeNodeCollisionObject : public collision::CollisionObject -{ -public: +//class ShapeNodeCollisionObject : public collision::CollisionObject +//{ +//public: - friend class collision::CollisionDetector; +// friend class collision::CollisionDetector; - // Documentation inherited - const Eigen::Isometry3d getTransform() const override; +// // Documentation inherited +// const Eigen::Isometry3d getTransform() const override; - // Documentation inherited - bool isEqual(const CollisionObject* other) const override; +// // Documentation inherited +// bool isEqual(const CollisionObject* other) const override; - /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject - dynamics::ShapeNode* getShapeNode(); +// /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject +// dynamics::ShapeNode* getShapeNode(); - /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject - const dynamics::ShapeNode* getShapeNode() const; +// /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject +// const dynamics::ShapeNode* getShapeNode() const; - /// Return BodyNode pointer associated with this ShapeNodeCollisionObject - dynamics::BodyNode* getBodyNode(); +// /// Return BodyNode pointer associated with this ShapeNodeCollisionObject +// dynamics::BodyNode* getBodyNode(); - /// Return BodyNode pointer associated with this ShapeNodeCollisionObject - const dynamics::BodyNode* getBodyNode() const; +// /// Return BodyNode pointer associated with this ShapeNodeCollisionObject +// const dynamics::BodyNode* getBodyNode() const; -protected: +//protected: - ShapeNodeCollisionObject( - const collision::CollisionDetectorPtr& collisionDetector, - const ShapePtr& shape, - const ShapeNodePtr& shapeNode); +// ShapeNodeCollisionObject( +// const collision::CollisionDetectorPtr& collisionDetector, +// const ShapePtr& shape, +// const ShapeNodePtr& shapeNode); - dynamics::ShapeNodePtr mShapeNode; +// dynamics::ShapeNodePtr mShapeNode; -}; +//}; -using ShapeNodeCollisionObjectPtr = std::shared_ptr; +//using ShapeNodeCollisionObjectPtr = std::shared_ptr; -/// Create a ShapeNodeCollisionObjectPtr given ShapeNode -ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( - const collision::CollisionDetectorPtr& collisionDetector, - const ShapeNodePtr& shapeNode); +///// Create a ShapeNodeCollisionObjectPtr given ShapeNode +//ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( +// const collision::CollisionDetectorPtr& collisionDetector, +// const ShapeNodePtr& shapeNode); -/// Create a ShapeNodeCollisionObjectPtr given Skeleton -std::vector createShapeFrameCollisionObjects( - const collision::CollisionDetectorPtr& collisionDetector, - const dynamics::SkeletonPtr& skel); +///// Create a ShapeNodeCollisionObjectPtr given Skeleton +//std::vector createShapeFrameCollisionObjects( +// const collision::CollisionDetectorPtr& collisionDetector, +// const dynamics::SkeletonPtr& skel); -/// Create a CollisionGroup given Skeleton -collision::CollisionGroupPtr createShapeFrameCollisionGroup( - const collision::CollisionDetectorPtr& collisionDetector, - const dynamics::SkeletonPtr& skel); +///// Create a CollisionGroup given Skeleton +//collision::CollisionGroupPtr createShapeFrameCollisionGroup( +// const collision::CollisionDetectorPtr& collisionDetector, +// const dynamics::SkeletonPtr& skel); } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 7938841f89198..87f341ee4ccd9 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -433,6 +433,18 @@ bool Frame::isShapeFrame() const return mAmShapeFrame; } +//============================================================================== +ShapeFrame* Frame::asShapeFrame() +{ + return nullptr; +} + +//============================================================================== +const ShapeFrame* Frame::asShapeFrame() const +{ + return nullptr; +} + //============================================================================== bool Frame::isWorld() const { diff --git a/dart/dynamics/Frame.h b/dart/dynamics/Frame.h index ceed644258338..58536662e5afd 100644 --- a/dart/dynamics/Frame.h +++ b/dart/dynamics/Frame.h @@ -232,6 +232,14 @@ class Frame : public virtual Entity /// Returns true if this Frame is a ShapeFrame bool isShapeFrame() const; + /// Convert 'this' into a ShapeFrame pointer if Frame is a ShapeFrame, + /// otherwise return nullptr + virtual ShapeFrame* asShapeFrame(); + + /// Convert 'const this' into a ShapeFrame pointer if Frame is a ShapeFrame, + /// otherwise return nullptr + virtual const ShapeFrame* asShapeFrame() const; + /// Returns true if this Frame is the World Frame bool isWorld() const; diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 5518b43bb029f..7a55acaca5440 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -36,6 +36,7 @@ #include "dart/dynamics/ShapeFrame.h" +#include "dart/dynamics/ShapeNode.h" #include "dart/renderer/OpenGLRenderInterface.h" namespace dart { @@ -312,11 +313,42 @@ size_t ShapeFrame::getVersion() const return mShapeFrameP.mVersion; } +//============================================================================== +ShapeFrame* ShapeFrame::asShapeFrame() +{ + return this; +} + +//============================================================================== +const ShapeFrame* ShapeFrame::asShapeFrame() const +{ + return this; +} + +//============================================================================== +bool ShapeFrame::isShapeNode() const +{ + return mAmShapeNode; +} + +//============================================================================== +ShapeNode* ShapeFrame::asShapeNode() +{ + return nullptr; +} + +//============================================================================== +const ShapeNode* ShapeFrame::asShapeNode() const +{ + return nullptr; +} + //============================================================================== ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) : common::AddonManager(), Entity(ConstructFrame), Frame(parent, ""), + mAmShapeNode(false), mShapeUpdatedSignal(), mRelativeTransformUpdatedSignal(), onShapeUpdated(mShapeUpdatedSignal), @@ -333,6 +365,7 @@ ShapeFrame::ShapeFrame(Frame* parent, : common::AddonManager(), Entity(ConstructFrame), Frame(parent, name), + mAmShapeNode(false), mShapeUpdatedSignal(), mRelativeTransformUpdatedSignal(), onShapeUpdated(mShapeUpdatedSignal), diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 6150fe69ca5ef..aa0e7a2d795c9 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -314,6 +314,23 @@ class ShapeFrame : // Documentation inherited size_t getVersion() const override; + // Documentation inherited + ShapeFrame* asShapeFrame() override; + + // Documentation inherited + const ShapeFrame* asShapeFrame() const override; + + /// Returns true if this Frame is a ShapeNode + bool isShapeNode() const; + + /// Convert 'this' into a ShapeNode pointer if ShapeFrame is a ShapeNode, + /// otherwise return nullptr + virtual ShapeNode* asShapeNode(); + + /// Convert 'const this' into a ShapeNode pointer if ShapeFrame is a ShapeNode, + /// otherwise return nullptr + virtual const ShapeNode* asShapeNode() const; + protected: /// Constructor @@ -327,6 +344,9 @@ class ShapeFrame : /// ShapeFrame properties Properties mShapeFrameP; + /// Contains whether or not this is a ShapeNode + bool mAmShapeNode; + /// Shape updated signal ShapeUpdatedSignal mShapeUpdatedSignal; diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index 27ca918d4e9cf..c3933fccc403b 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -313,6 +313,18 @@ const math::Jacobian& ShapeNode::getJacobianClassicDeriv() const return mWorldJacobianClassicDeriv; } +//============================================================================== +ShapeNode* ShapeNode::asShapeNode() +{ + return this; +} + +//============================================================================== +const ShapeNode* ShapeNode::asShapeNode() const +{ + return this; +} + //============================================================================== ShapeNode::ShapeNode(BodyNode* bodyNode, const Properties& properties) : Entity(ConstructFrame), diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index a6d88aaed7334..7ef6e10e1b224 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -226,6 +226,12 @@ class ShapeNode : public virtual FixedFrame, /// \} + // Documentation inherited + ShapeNode* asShapeNode() override; + + // Documentation inherited + const ShapeNode* asShapeNode() const override; + protected: /// Constructor used by the Skeleton class diff --git a/dart/dynamics/SimpleFrame.cpp b/dart/dynamics/SimpleFrame.cpp index 403823fc1d16f..6386a364e1ef2 100644 --- a/dart/dynamics/SimpleFrame.cpp +++ b/dart/dynamics/SimpleFrame.cpp @@ -146,6 +146,20 @@ void SimpleFrame::setRelativeTransform( notifyTransformUpdate(); } +//============================================================================== +void SimpleFrame::setRelativeTranslation(const Eigen::Vector3d& _newTranslation) +{ + mRelativeTf.translation() = _newTranslation; + notifyTransformUpdate(); +} + +//============================================================================== +void SimpleFrame::setRelativeRotation(const Eigen::Matrix3d& _newRotation) +{ + mRelativeTf.linear() = _newRotation; + notifyTransformUpdate(); +} + //============================================================================== void SimpleFrame::setTransform(const Eigen::Isometry3d& _newTransform, const Frame* _withRespectTo) @@ -154,6 +168,23 @@ void SimpleFrame::setTransform(const Eigen::Isometry3d& _newTransform, _withRespectTo->getTransform(getParentFrame()) * _newTransform); } +//============================================================================== +void SimpleFrame::setTranslation(const Eigen::Vector3d& _newTranslation, + const Frame* _withRespectTo) +{ + setRelativeTranslation( + _withRespectTo->getTransform(getParentFrame()) * _newTranslation); +} + +//============================================================================== +void SimpleFrame::setRotation(const Eigen::Matrix3d& _newRotation, + const Frame* _withRespectTo) +{ + setRelativeRotation( + _withRespectTo->getTransform(getParentFrame()).linear() + * _newRotation); +} + //============================================================================== const Eigen::Isometry3d& SimpleFrame::getRelativeTransform() const { diff --git a/dart/dynamics/SimpleFrame.h b/dart/dynamics/SimpleFrame.h index c2d56e46b73c5..408bfa31b624c 100644 --- a/dart/dynamics/SimpleFrame.h +++ b/dart/dynamics/SimpleFrame.h @@ -104,12 +104,30 @@ class SimpleFrame : public Detachable, public ShapeFrame /// Set the relative transform of this SimpleFrame void setRelativeTransform(const Eigen::Isometry3d& _newRelTransform); + /// Set the relative translation of this SimpleFrame + void setRelativeTranslation(const Eigen::Vector3d& _newTranslation); + + /// Set the relative rotation of this SimpleFrame + void setRelativeRotation(const Eigen::Matrix3d& _newRotation); + /// Set the transform of this SimpleFrame so that its transform with respect /// to Frame _withRespectTo is equal to _newTransform. Note that the parent /// Frame of this SimpleFrame will not be changed. void setTransform(const Eigen::Isometry3d& _newTransform, const Frame* _withRespectTo = Frame::World()); + /// Set the translation of this SimpleFrame so that its translation with + /// respect to Frame _withRespectTo is equal to _newTranslation. Note that the + /// parent Frame of this SimpleFrame will not be changed. + void setTranslation(const Eigen::Vector3d& _newTranslation, + const Frame* _withRespectTo = Frame::World()); + + /// Set the rotation of this SimpleFrame so that its rotation with respect + /// to Frame _withRespectTo is equal to _newRotation. Note that the parent + /// Frame of this SimpleFrame will not be changed. + void setRotation(const Eigen::Matrix3d& _newRotation, + const Frame* _withRespectTo = Frame::World()); + // Documentation inherited const Eigen::Isometry3d& getRelativeTransform() const; diff --git a/osgDart/WorldNode.cpp b/osgDart/WorldNode.cpp index 9d54c5cd1713d..09ed9ec952ab7 100644 --- a/osgDart/WorldNode.cpp +++ b/osgDart/WorldNode.cpp @@ -267,9 +267,7 @@ void WorldNode::refreshShapeFrameNode(dart::dynamics::Frame* frame) return; } - dart::dynamics::ShapeFrame* shapeFrame = - dynamic_cast(frame); - if(!shapeFrame) + if(!frame->isShapeFrame()) { dtwarn << "[WorldNode::refreshShapeFrameNode] Frame named [" << frame->getName() << "] (" << frame << ") claims to be a " @@ -278,7 +276,8 @@ void WorldNode::refreshShapeFrameNode(dart::dynamics::Frame* frame) return; } - osg::ref_ptr node = new ShapeFrameNode(shapeFrame, this); + osg::ref_ptr node = new ShapeFrameNode(frame->asShapeFrame(), + this); it->second = node; addChild(node); } diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index cf1e0527bfc7a..1ea34060f4e8a 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -215,7 +215,7 @@ class MyWindow : public dart::gui::SimWindow // collision with something auto collisionEngine = mWorld->getConstraintSolver()->getCollisionDetector(); auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); - auto newGroup = createShapeFrameCollisionGroup(collisionEngine, object); + auto newGroup = collisionEngine->createCollisionGroup(object.get()); dart::collision::Option option; dart::collision::Result result; diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index c0e2da8b8ae4b..acc71cf604957 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -266,7 +266,7 @@ class MyWindow : public dart::gui::SimWindow // something auto collisionEngine = mWorld->getConstraintSolver()->getCollisionDetector(); auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); - auto newGroup = createShapeFrameCollisionGroup(collisionEngine, newDomino); + auto newGroup = collisionEngine->createCollisionGroup(newDomino.get()); dart::collision::Option option; dart::collision::Result result; diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 0390f3f11e62a..3c11f05057f57 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -514,71 +514,62 @@ TEST_F(COLLISION, FCL_BOX_BOX) //============================================================================== template -void testFreeCollisionObjects() +void testSimpleFrames() { auto cd = CollisionDetectorType::create(); + auto simpleFrame1 = std::make_shared(Frame::World()); + auto simpleFrame2 = std::make_shared(Frame::World()); + auto simpleFrame3 = std::make_shared(Frame::World()); + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); - // Create collision objects in different ways - auto obj1 = cd->template createCollisionObject(shape1); - auto obj2 = FreeCollisionObject::create(cd, shape2); - auto obj3 = FreeCollisionObject::create(cd, shape3); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + simpleFrame3->setShape(shape3); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + auto group3 = cd->createCollisionGroup(simpleFrame3.get()); - collision::CollisionGroupPtr group1(new collision::CollisionGroup(cd)); - group1->addCollisionObject(obj1); - collision::CollisionGroupPtr group2(new collision::CollisionGroup(cd)); - group2->addCollisionObject(obj2); - group2->addCollisionObject(obj3); + auto groupAll = cd->createCollisionGroup(); + groupAll->unionGroup(group1); + groupAll->unionGroup(group2); + groupAll->unionGroup(group3); collision::Option option; collision::Result result; - obj1->setTranslation(Eigen::Vector3d::Zero()); - obj2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); - obj3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); - EXPECT_FALSE(obj1->detect(obj2.get(), option, result)); - EXPECT_FALSE(obj2->detect(obj3.get(), option, result)); - EXPECT_FALSE(obj1->detect(group2.get(), option, result)); - EXPECT_FALSE(group1->detect(obj3.get(), option, result)); - if (cd->getType() != "Bullet") - { - EXPECT_FALSE(group1->detect(group2.get(), option, result)); - EXPECT_FALSE(group2->detect(option, result)); - } - - obj1->setTranslation(Eigen::Vector3d::Zero()); - obj2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); - obj3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); - EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); - EXPECT_TRUE(obj2->detect(obj3.get(), option, result)); - EXPECT_TRUE(obj1->detect(group2.get(), option, result)); - if (cd->getType() != "Bullet") - EXPECT_TRUE(group1->detect(group2.get(), option, result)); - EXPECT_TRUE(group2->detect(option, result)); - EXPECT_TRUE(group2->detect(option, result)); - - for (auto contact : result.contacts) - { - auto freeCollObj1 = dynamic_cast( - contact.collisionObject1); - auto freeCollObj2 = dynamic_cast( - contact.collisionObject2); - - EXPECT_NE(freeCollObj1, nullptr); - EXPECT_NE(freeCollObj2, nullptr); - } + EXPECT_FALSE(group1->detect(option, result)); + EXPECT_FALSE(group2->detect(option, result)); + EXPECT_FALSE(group3->detect(option, result)); + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); + EXPECT_FALSE(group1->detect(group2.get(), option, result)); + EXPECT_FALSE(group1->detect(group3.get(), option, result)); + EXPECT_FALSE(group2->detect(group3.get(), option, result)); + EXPECT_FALSE(groupAll->detect(option, result)); + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); + simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(group2->detect(group3.get(), option, result)); + EXPECT_TRUE(groupAll->detect(option, result)); } //============================================================================== TEST_F(COLLISION, FreeCollisionObjects) { - testFreeCollisionObjects(); - testFreeCollisionObjects(); - testFreeCollisionObjects(); - testFreeCollisionObjects(); + testSimpleFrames(); + testSimpleFrames(); + testSimpleFrames(); +// testSimpleFrames(); } //============================================================================== @@ -603,36 +594,19 @@ void testBodyNodes() collision::Option option; collision::Result result; - auto obj1 = cd->template createCollisionObject( - boxShapeNode1->getShape(), boxShapeNode1); - auto obj2 = cd->template createCollisionObject( - boxShapeNode2->getShape(), boxShapeNode2); - - EXPECT_TRUE(obj1->detect(obj2.get(), option, result)); - EXPECT_TRUE(cd->detect(obj1.get(), obj2.get(), option, result)); - - auto obj3 = cd->template createCollisionObject( - boxShapeNode2->getShape(), boxShapeNode2); - - EXPECT_FALSE(obj1->isEqual(obj2.get())); - EXPECT_FALSE(obj2->isEqual(obj1.get())); - - EXPECT_FALSE(obj1->isEqual(obj3.get())); - EXPECT_FALSE(obj3->isEqual(obj1.get())); + auto group1 = cd->createCollisionGroup(boxShapeNode1); + auto group2 = cd->createCollisionGroup(boxShapeNode2); - EXPECT_TRUE(obj2->isEqual(obj3.get())); - EXPECT_TRUE(obj3->isEqual(obj2.get())); +// EXPECT_TRUE(group1->detect(group2.get(), option, result)); +// EXPECT_TRUE(cd->detect(group1.get(), group2.get(), option, result)); - collision::CollisionGroupPtr group1(new collision::CollisionGroup(cd)); - group1->addCollisionObject(obj2); - collision::CollisionGroupPtr group2(new collision::CollisionGroup(cd)); - group2->addCollisionObject(obj3); +// auto group3 = cd->createCollisionGroup(boxShapeNode2); - EXPECT_EQ(group1->getCollisionObjects().size(), 1u); - EXPECT_EQ(group2->getCollisionObjects().size(), 1u); +// EXPECT_EQ(group1->getCollisionObjects().size(), 1u); +// EXPECT_EQ(group2->getCollisionObjects().size(), 1u); - group1->unionGroup(group2); - EXPECT_EQ(group1->getCollisionObjects().size(), 1u); +// group1->unionGroup(group2); +// EXPECT_EQ(group1->getCollisionObjects().size(), 1u); } //============================================================================== From 0de9993f1e37f83c17608ebf12c4901e55a9cd16 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 23 Mar 2016 22:48:54 -0400 Subject: [PATCH 21/67] Merge FCLMeshCollisionDetector into FCLCollisionDetector --- dart/collision/CollisionDetector.cpp | 98 -- dart/collision/CollisionDetector.h | 33 +- dart/collision/CollisionGroup.cpp | 6 + dart/collision/CollisionGroup.h | 3 + dart/collision/SmartPointer.h | 1 - .../bullet/BulletCollisionDetector.cpp | 247 ++-- .../bullet/BulletCollisionDetector.h | 16 +- dart/collision/dart/DARTCollisionDetector.cpp | 48 +- dart/collision/dart/DARTCollisionDetector.h | 16 +- dart/collision/fcl/FCLCollisionDetector.cpp | 555 ++++++-- dart/collision/fcl/FCLCollisionDetector.h | 53 +- .../fcl/FCLMeshCollisionDetector.cpp | 1173 ----------------- dart/collision/fcl/FCLMeshCollisionDetector.h | 125 -- dart/collision/fcl/FCLMeshCollisionGroup.cpp | 146 -- dart/collision/fcl/FCLMeshCollisionGroup.h | 111 -- dart/collision/fcl/FCLMeshCollisionObject.cpp | 130 -- dart/collision/fcl/FCLMeshCollisionObject.h | 87 -- dart/constraint/ConstraintSolver.cpp | 30 +- dart/dynamics/CollisionDetector.cpp | 124 -- dart/dynamics/CollisionDetector.h | 52 - dart/utils/SkelParser.cpp | 26 +- unittests/testCollision.cpp | 196 ++- 22 files changed, 846 insertions(+), 2430 deletions(-) delete mode 100644 dart/collision/fcl/FCLMeshCollisionDetector.cpp delete mode 100644 dart/collision/fcl/FCLMeshCollisionDetector.h delete mode 100644 dart/collision/fcl/FCLMeshCollisionGroup.cpp delete mode 100644 dart/collision/fcl/FCLMeshCollisionGroup.h delete mode 100644 dart/collision/fcl/FCLMeshCollisionObject.cpp delete mode 100644 dart/collision/fcl/FCLMeshCollisionObject.h diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index edaefdc01458d..dd45d7f4e0203 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -78,90 +78,6 @@ std::shared_ptr CollisionDetector::createCollisionGroup( return group; } -////============================================================================== -//std::shared_ptr CollisionDetector::createCollisionGroup( -// const std::vector& objects) -//{ -// return std::make_shared(shared_from_this(), objects); -//} - -////============================================================================== -//bool CollisionDetector::detect( -// CollisionObject* object1, CollisionObject* object2, -// const Option& option, Result& result) -//{ -// assert(object1->getCollisionDetector() == object2->getCollisionDetector()); - -// object1->updateEngineData(); -// object2->updateEngineData(); - -// return detect(object1->getEngineData(), object2->getEngineData(), -// option, result); -//} - -////============================================================================== -//bool CollisionDetector::detect( -// CollisionObject* object, CollisionGroup* group, -// const Option& option, Result& result) -//{ -// assert(object->getCollisionDetector() == group->getCollisionDetector()); - -// object->updateEngineData(); -// group->updateEngineData(); - -// return detect(object->getEngineData(), group->getEngineData(), -// option, result); -//} - -////============================================================================== -//bool CollisionDetector::detect( -// CollisionGroup* group, CollisionObject* object, -// const Option& option, Result& result) -//{ -// return detect(object, group, option, result); -//} - -////============================================================================== -//bool CollisionDetector::detect( -// CollisionGroup* group, const Option& option, Result& result) -//{ -// group->update(); - -// return detect(group, option, result); -//} - -////============================================================================== -//bool CollisionDetector::detect( -// CollisionGroup* group1, CollisionGroup* group2, -// const Option& option, Result& result) -//{ -// assert(group1->getCollisionDetector() == group2->getCollisionDetector()); - -// group1->updateEngineData(); -// group2->updateEngineData(); - -// return detect(group1->getEngineData(), group2->getEngineData(), -// option, result); -//} - -//============================================================================== -struct is_collision_object_equal - : public std::unary_function -{ - explicit is_collision_object_equal(const CollisionObject* object) - : mObject(object) {} - - bool operator() (const WeakCollisionObjectPtr& arg) - { - auto locked = arg.lock(); - assert(locked); - - return mObject == locked.get(); - } - - const CollisionObject* mObject; -}; - //============================================================================== CollisionObject* CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) @@ -214,19 +130,5 @@ void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) } } -//============================================================================== -bool CollisionDetector::hasCollisionObject(const CollisionObject* collObj) const -{ - if (!collObj) - return false; - - return true; -// return std::find_if( -// mCollisionObjectMap.begin(), mCollisionObjectMap.end(), -// [&](const CollisionObjectPair& pair) -// { return pair.second.first.get() == collObj; }) -// != mCollisionObjectMap.end(); -} - } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 53a1e276fcf09..8f0158836f8c3 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -82,25 +82,13 @@ class CollisionDetector : public std::enable_shared_from_this std::shared_ptr createCollisionGroup( dynamics::Skeleton* skeleton); -// /// Perform collision detection for object1-object2. -// bool detect(CollisionObject* object1, CollisionObject* object2, -// const Option& option, Result& result); - -// /// Perform collision detection for object-group. -// bool detect(CollisionObject* object, CollisionGroup* group, -// const Option& option, Result& result); - -// /// Identical with detect(object, group, option, result) -// bool detect(CollisionGroup* group, CollisionObject* object, -// const Option& option, Result& result); - /// Perform collision detection for group. -// bool detect(CollisionGroup* group, -// const Option& option, Result& result); + virtual bool detect(CollisionGroup* group, + const Option& option, Result& result) = 0; -// /// Perform collision detection for group1-group2. -// bool detect(CollisionGroup* group1, CollisionGroup* group2, -// const Option& option, Result& result); + /// Perform collision detection for group1-group2. + virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) = 0; protected: @@ -125,17 +113,6 @@ class CollisionDetector : public std::enable_shared_from_this /// will be destroyed if no CollisionGroup holds it. void reclaimCollisionObject(const CollisionObject* shapeFrame); - /// Return true if collisionObject is created from this CollisionDetector - bool hasCollisionObject(const CollisionObject* collisionObject) const; - - /// Perform collision detection for group. - virtual bool detect(CollisionGroup* group, - const Option& option, Result& result) = 0; - - /// Perform collision detection for group1-group2. - virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) = 0; - protected: using CollisionObjectMapValue diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 8e2b703bf54b8..8c3fa139fd130 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -163,6 +163,12 @@ bool CollisionGroup::hasShapeFrame( != mShapeFrames.end(); } +//============================================================================== +size_t CollisionGroup::getNumShapeFrames() const +{ + return mShapeFrames.size(); +} + //============================================================================== void CollisionGroup::unionGroup(const CollisionGroupPtr& other) { diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index f9b4c64f10f72..1c020cd6d074d 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -82,6 +82,9 @@ class CollisionGroup /// Return true if this CollisionGroup contains shapeFrame bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const; + /// Return number of ShapeFrames added to this CollisionGroup + size_t getNumShapeFrames() const; + /// Merge other CollisionGroup into this CollisionGroup void unionGroup(const CollisionGroupPtr& other); diff --git a/dart/collision/SmartPointer.h b/dart/collision/SmartPointer.h index a2cee4585bda4..3736466e8fc0b 100644 --- a/dart/collision/SmartPointer.h +++ b/dart/collision/SmartPointer.h @@ -45,7 +45,6 @@ namespace collision { DART_COMMON_MAKE_SHARED_WEAK(CollisionDetector) DART_COMMON_MAKE_SHARED_WEAK(FCLCollisionDetector) -DART_COMMON_MAKE_SHARED_WEAK(FCLMeshCollisionDetector) DART_COMMON_MAKE_SHARED_WEAK(DARTCollisionDetector) #ifdef HAVE_BULLET_COLLISION DART_COMMON_MAKE_SHARED_WEAK(BulletCollisionDetector) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index f0aa031c63cde..06b2fdf9dabc5 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -121,6 +121,12 @@ struct BulletContactResultCallback : btCollisionWorld::ContactResultCallback Result& mResult; }; +Contact convertContact(const btManifoldPoint& bulletManifoldPoint, + const BulletCollisionObject::UserData* userData1, + const BulletCollisionObject::UserData* userData2); + +void convertContacts(btCollisionWorld* collWorld, Result& result); + } // anonymous namespace @@ -172,6 +178,72 @@ std::shared_ptr BulletCollisionDetector::createCollisionGroup( shapeFrames); } +//============================================================================== +bool BulletCollisionDetector::detect( + CollisionGroup* group, const Option& option, Result& result) +{ + result.contacts.clear(); + + if (!group) + return false; + + if (group->getCollisionDetector()->getType() + != BulletCollisionDetector::getTypeStatic()) + { + return false; + } + + group->update(); + + auto castedData = static_cast(group); + auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); + auto bulletPairCache = bulletCollisionWorld->getPairCache(); + auto filterCallback + = new BulletOverlapFilterCallback(option.collisionFilter.get()); + + bulletPairCache->setOverlapFilterCallback(filterCallback); + bulletCollisionWorld->performDiscreteCollisionDetection(); + + convertContacts(bulletCollisionWorld, result); + + return !result.contacts.empty(); +} + +//============================================================================== +bool BulletCollisionDetector::detect( + CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) +{ + result.contacts.clear(); + + if (!group1 || !group2) + return false; + + if (group1->getCollisionDetector()->getType() + != BulletCollisionDetector::getTypeStatic()) + { + return false; + } + + if (group2->getCollisionDetector()->getType() + != BulletCollisionDetector::getTypeStatic()) + { + return false; + } + + auto castedData1 = static_cast(group1); + auto castedData2 = static_cast(group2); + + auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); + auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); + +// BulletCollisionData collData(&option, &result); +// bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); + // TODO(JS) + + return !result.contacts.empty(); +} + //============================================================================== std::unique_ptr BulletCollisionDetector::createCollisionObject( @@ -254,126 +326,6 @@ void BulletCollisionDetector::reclaimBulletCollisionGeometry( } } -//============================================================================== -Contact convertContact(const btManifoldPoint& bulletManifoldPoint, - const BulletCollisionObject::UserData* userData1, - const BulletCollisionObject::UserData* userData2) -{ - assert(userData1); - assert(userData2); - - Contact contact; - - contact.point = convertVector3(bulletManifoldPoint.getPositionWorldOnA()); - contact.normal = convertVector3(-bulletManifoldPoint.m_normalWorldOnB); - contact.penetrationDepth = -bulletManifoldPoint.m_distance1; - contact.collisionObject1 = userData1->collisionObject; - contact.collisionObject2 = userData2->collisionObject; - - return contact; -} - -//============================================================================== -void convertContacts(btCollisionWorld* collWorld, Result& result) -{ - assert(collWorld); - - auto dispatcher = collWorld->getDispatcher(); - assert(dispatcher); - - auto numManifolds = dispatcher->getNumManifolds(); - - for (auto i = 0; i < numManifolds; ++i) - { - auto contactManifold = dispatcher->getManifoldByIndexInternal(i); - const auto bulletCollObj0 = contactManifold->getBody0(); - const auto bulletCollObj1 = contactManifold->getBody1(); - - auto userPointer0 = bulletCollObj0->getUserPointer(); - auto userPointer1 = bulletCollObj1->getUserPointer(); - - auto userDataA - = static_cast(userPointer1); - auto userDataB - = static_cast(userPointer0); - - auto numContacts = contactManifold->getNumContacts(); - - for (auto j = 0; j < numContacts; ++j) - { - auto& cp = contactManifold->getContactPoint(j); - - result.contacts.push_back(convertContact(cp, userDataA, userDataB)); - } - } -} - -//============================================================================== -bool BulletCollisionDetector::detect( - CollisionGroup* group, const Option& option, Result& result) -{ - result.contacts.clear(); - - if (!group) - return false; - - if (group->getCollisionDetector()->getType() - != BulletCollisionDetector::getTypeStatic()) - { - return false; - } - - group->update(); - - auto castedData = static_cast(group); - auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); - auto bulletPairCache = bulletCollisionWorld->getPairCache(); - auto filterCallback - = new BulletOverlapFilterCallback(option.collisionFilter.get()); - - bulletPairCache->setOverlapFilterCallback(filterCallback); - bulletCollisionWorld->performDiscreteCollisionDetection(); - - convertContacts(bulletCollisionWorld, result); - - return !result.contacts.empty(); -} - -//============================================================================== -bool BulletCollisionDetector::detect( - CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) -{ - result.contacts.clear(); - - if (!group1 || !group2) - return false; - - if (group1->getCollisionDetector()->getType() - != BulletCollisionDetector::getTypeStatic()) - { - return false; - } - - if (group2->getCollisionDetector()->getType() - != BulletCollisionDetector::getTypeStatic()) - { - return false; - } - - auto castedData1 = static_cast(group1); - auto castedData2 = static_cast(group2); - - auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); - auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); - -// BulletCollisionData collData(&option, &result); -// bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); - // TODO(JS) - - return !result.contacts.empty(); -} - //============================================================================== BulletCollisionDetector::BulletCollsionPack BulletCollisionDetector::createMesh( @@ -587,6 +539,61 @@ btScalar BulletContactResultCallback::addSingleResult( return 1.0f; } + +//============================================================================== +Contact convertContact(const btManifoldPoint& bulletManifoldPoint, + const BulletCollisionObject::UserData* userData1, + const BulletCollisionObject::UserData* userData2) +{ + assert(userData1); + assert(userData2); + + Contact contact; + + contact.point = convertVector3(bulletManifoldPoint.getPositionWorldOnA()); + contact.normal = convertVector3(-bulletManifoldPoint.m_normalWorldOnB); + contact.penetrationDepth = -bulletManifoldPoint.m_distance1; + contact.collisionObject1 = userData1->collisionObject; + contact.collisionObject2 = userData2->collisionObject; + + return contact; +} + +//============================================================================== +void convertContacts(btCollisionWorld* collWorld, Result& result) +{ + assert(collWorld); + + auto dispatcher = collWorld->getDispatcher(); + assert(dispatcher); + + auto numManifolds = dispatcher->getNumManifolds(); + + for (auto i = 0; i < numManifolds; ++i) + { + auto contactManifold = dispatcher->getManifoldByIndexInternal(i); + const auto bulletCollObj0 = contactManifold->getBody0(); + const auto bulletCollObj1 = contactManifold->getBody1(); + + auto userPointer0 = bulletCollObj0->getUserPointer(); + auto userPointer1 = bulletCollObj1->getUserPointer(); + + auto userDataA + = static_cast(userPointer1); + auto userDataB + = static_cast(userPointer0); + + auto numContacts = contactManifold->getNumContacts(); + + for (auto j = 0; j < numContacts; ++j) + { + auto& cp = contactManifold->getContactPoint(j); + + result.contacts.push_back(convertContact(cp, userDataA, userDataB)); + } + } +} + } // anonymous namespace } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index c0def1074edd4..4677503f599f4 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -75,7 +75,13 @@ class BulletCollisionDetector : public CollisionDetector std::shared_ptr createCollisionGroup( const std::vector& shapeFrames) override; - using CollisionDetector::detect; + // Documentation inherited + bool detect(CollisionGroup* group, + const Option& option, Result& result) override; + + // Documentation inherited + bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; protected: @@ -110,14 +116,6 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited void reclaimBulletCollisionGeometry(const dynamics::ConstShapePtr& shape); - // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; - - // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; - BulletCollsionPack createMesh( const Eigen::Vector3d& scale, const aiScene* mesh); diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 912f711142f3e..27fb5b7aad0f2 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -99,30 +99,6 @@ std::shared_ptr DARTCollisionDetector::createCollisionGroup( return std::make_shared(shared_from_this(), shapeFrames); } -//============================================================================== -std::unique_ptr DARTCollisionDetector::createCollisionObject( - const dynamics::ShapeFrame* shapeFrame) -{ - auto collObj = new DARTCollisionObject(this, shapeFrame); - - mDARTCollisionObjects.push_back(collObj); - - return std::unique_ptr(collObj); -} - -//============================================================================== -void DARTCollisionDetector::notifyDestroyingCollisionObject( - CollisionObject* collObj) -{ - if (!collObj) - return; - - auto casted = static_cast(collObj); - mDARTCollisionObjects.erase( - std::remove(mDARTCollisionObjects.begin(), mDARTCollisionObjects.end(), - casted), mDARTCollisionObjects.end()); -} - //============================================================================== bool DARTCollisionDetector::detect( CollisionGroup* group, @@ -190,6 +166,30 @@ bool DARTCollisionDetector::detect( return !result.contacts.empty(); } +//============================================================================== +std::unique_ptr DARTCollisionDetector::createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) +{ + auto collObj = new DARTCollisionObject(this, shapeFrame); + + mDARTCollisionObjects.push_back(collObj); + + return std::unique_ptr(collObj); +} + +//============================================================================== +void DARTCollisionDetector::notifyDestroyingCollisionObject( + CollisionObject* collObj) +{ + if (!collObj) + return; + + auto casted = static_cast(collObj); + mDARTCollisionObjects.erase( + std::remove(mDARTCollisionObjects.begin(), mDARTCollisionObjects.end(), + casted), mDARTCollisionObjects.end()); +} + namespace { diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 08b1cd0d2fe93..7e2f5e0d6c527 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -68,7 +68,13 @@ class DARTCollisionDetector : public CollisionDetector std::shared_ptr createCollisionGroup( const std::vector& shapeFrames) override; - using CollisionDetector::detect; + // Documentation inherited + bool detect(CollisionGroup* group, + const Option& option, Result& result) override; + + // Documentation inherited + bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; protected: @@ -82,14 +88,6 @@ class DARTCollisionDetector : public CollisionDetector // Documentation inherited void notifyDestroyingCollisionObject(CollisionObject* collObj) override; - // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; - - // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; - protected: std::vector mDARTCollisionObjects; diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 2ee026ce665cb..b3341e74272b8 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -52,6 +52,7 @@ #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObject.h" #include "dart/collision/fcl/FCLCollisionGroup.h" +#include "dart/collision/fcl/tri_tri_intersection_test.h" #include "dart/dynamics/ShapeFrame.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" @@ -71,11 +72,35 @@ bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); -void postProcess(const fcl::CollisionResult& fclResult, +void postProcessFCL(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, Result& result); +void postProcessDART(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result); + +int evalContactPosition(const fcl::Contact& fclContact, + const fcl::BVHModel* mesh1, + const fcl::BVHModel* mesh2, + const fcl::Transform3f& transform1, + const fcl::Transform3f& transform2, + Eigen::Vector3d* contactPosition); + +bool isColinear(const Eigen::Vector3d& pos1, + const Eigen::Vector3d& pos2, + const Eigen::Vector3d& pos3, + double tol); + +int FFtest( + const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, + const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, + fcl::Vec3f* res1, fcl::Vec3f* res2); + +double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); + void convertOption(const Option& fclOption, fcl::CollisionRequest& request); Contact convertContact(const fcl::Contact& fclContact, @@ -101,6 +126,11 @@ struct FCLCollisionCallbackData /// Collision result of DART Result* mResult; + FCLCollisionDetector::PrimitiveShape_t mPrimitiveShapeType; + + FCLCollisionDetector::ContactPointComputationMethod_t + mContactPointComputationMethod; + /// Whether the collision iteration can stop bool done; @@ -108,10 +138,16 @@ struct FCLCollisionCallbackData FCLCollisionCallbackData( FCLCollisionDetector* collisionDetector, const Option* option = nullptr, - Result* result = nullptr) + Result* result = nullptr, + FCLCollisionDetector::PrimitiveShape_t type + = FCLCollisionDetector::MESH, + FCLCollisionDetector::ContactPointComputationMethod_t method + = FCLCollisionDetector::DART) : mFclCollisionDetector(collisionDetector), mOption(option), mResult(result), + mPrimitiveShapeType(type), + mContactPointComputationMethod(method), done(false) { if (mOption) @@ -119,8 +155,6 @@ struct FCLCollisionCallbackData } }; -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - //============================================================================== // Create a cube mesh for collision detection template @@ -371,8 +405,6 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) return model; } -#endif - #if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) //============================================================================== @@ -538,7 +570,8 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) //============================================================================== boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape) + const dynamics::ConstShapePtr& shape, + FCLCollisionDetector::PrimitiveShape_t type) { using dynamics::Shape; using dynamics::BoxShape; @@ -559,11 +592,10 @@ boost::shared_ptr createFCLCollisionGeometry( auto box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset(createCube(size[0], size[1], size[2])); -#else - fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); -#endif + if (FCLCollisionDetector::PRIMITIVE == type) + fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); + else + fclCollGeom.reset(createCube(size[0], size[1], size[2])); break; } @@ -574,19 +606,22 @@ boost::shared_ptr createFCLCollisionGeometry( auto ellipsoid = static_cast(shape.get()); const Eigen::Vector3d& size = ellipsoid->getSize(); - if (ellipsoid->isSphere()) + if (FCLCollisionDetector::PRIMITIVE == type) { - fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); + if (ellipsoid->isSphere()) + { + fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); + } + else + { + fclCollGeom.reset( + new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); + } } else { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) fclCollGeom.reset( - createEllipsoid(size[0], size[1], size[2])); -#else - fclCollGeom.reset( - new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); -#endif + createEllipsoid(size[0], size[1], size[2])); } break; @@ -599,34 +634,41 @@ boost::shared_ptr createFCLCollisionGeometry( const auto radius = cylinder->getRadius(); const auto height = cylinder->getHeight(); -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); -#else - fclCollGeom.reset(new fcl::Cylinder(radius, height)); -#endif - // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 - // returns single contact point for cylinder yet. + if (FCLCollisionDetector::PRIMITIVE == type) + { + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); + // TODO(JS): We still need to use mesh for cylinder because FCL 0.4.0 + // returns single contact point for cylinder yet. Once FCL support + // multiple contact points then above code will be replaced by: + // fclCollGeom.reset(new fcl::Cylinder(radius, height)); + } + else + { + fclCollGeom.reset(createCylinder( + radius, radius, height, 16, 16)); + } break; } case Shape::PLANE: { -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,3) - fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); - - dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " - << "FCLCollisionDetector. We create a thin box mesh insted, where " - << "the size is [1000 0 1000].\n"; -#else - - assert(dynamic_cast(shape.get())); - auto plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - fclCollGeom.reset( - new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); -#endif + if (FCLCollisionDetector::PRIMITIVE == type) + { + assert(dynamic_cast(shape.get())); + auto plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + fclCollGeom.reset( + new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); + } + else + { + fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); + dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " + << "FCLCollisionDetector. We create a thin box mesh insted, where " + << "the size is [1000 0 1000].\n"; + } break; } @@ -716,6 +758,109 @@ std::shared_ptr FCLCollisionDetector::createCollisionGroup( return std::make_shared(shared_from_this(), shapeFrames); } +//============================================================================== +bool FCLCollisionDetector::detect( + CollisionGroup* group, const Option& option, Result& result) +{ + result.contacts.clear(); + + if (!group) + return false; + + if (group->getCollisionDetector()->getType() + != FCLCollisionDetector::getTypeStatic()) + { + return false; + } + + group->update(); + + auto castedData = static_cast(group); + auto broadPhaseAlg = castedData->getFCLCollisionManager(); + + FCLCollisionCallbackData collData( + this, &option, &result, mPrimitiveShapeType, + mContactPointComputationMethod); + broadPhaseAlg->collide(&collData, collisionCallback); + + return !result.contacts.empty(); +} + +//============================================================================== +bool FCLCollisionDetector::detect( + CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) +{ + result.contacts.clear(); + + if (!group1 || !group2) + return false; + + if (group1->getCollisionDetector()->getType() + != FCLCollisionDetector::getTypeStatic()) + { + return false; + } + + if (group2->getCollisionDetector()->getType() + != FCLCollisionDetector::getTypeStatic()) + { + return false; + } + + group1->update(); + group2->update(); + + auto casted1 = static_cast(group1); + auto casted2 = static_cast(group2); + + auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); + auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); + + FCLCollisionCallbackData collData( + this, &option, &result, mPrimitiveShapeType, + mContactPointComputationMethod); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); + + return !result.contacts.empty(); +} + +//============================================================================== +void FCLCollisionDetector::setPrimitiveShapeType( + FCLCollisionDetector::PrimitiveShape_t type) +{ + mPrimitiveShapeType = type; +} + +//============================================================================== +FCLCollisionDetector::PrimitiveShape_t +FCLCollisionDetector::getPrimitiveShapeType() const +{ + return mPrimitiveShapeType; +} + +//============================================================================== +void FCLCollisionDetector::setContactPointComputationMethod( + FCLCollisionDetector::ContactPointComputationMethod_t method) +{ + mContactPointComputationMethod = method; +} + +//============================================================================== +FCLCollisionDetector::ContactPointComputationMethod_t +FCLCollisionDetector::getContactPointComputationMethod() const +{ + return mContactPointComputationMethod; +} + +//============================================================================== +FCLCollisionDetector::FCLCollisionDetector() + : mPrimitiveShapeType(MESH), + mContactPointComputationMethod(DART) +{ + // Do nothing +} + //============================================================================== std::unique_ptr FCLCollisionDetector::createCollisionObject(const dynamics::ShapeFrame* shapeFrame) @@ -773,7 +918,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } else { - fclCollGeom = createFCLCollisionGeometry(shape); + fclCollGeom = createFCLCollisionGeometry(shape, mPrimitiveShapeType); mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); } @@ -797,69 +942,6 @@ void FCLCollisionDetector::reclaimFCLCollisionGeometry( mShapeMap.erase(search); } -//============================================================================== -bool FCLCollisionDetector::detect( - CollisionGroup* group, const Option& option, Result& result) -{ - result.contacts.clear(); - - if (!group) - return false; - - if (group->getCollisionDetector()->getType() - != FCLCollisionDetector::getTypeStatic()) - { - return false; - } - - group->update(); - - auto castedData = static_cast(group); - auto broadPhaseAlg = castedData->getFCLCollisionManager(); - - FCLCollisionCallbackData collData(this, &option, &result); - broadPhaseAlg->collide(&collData, collisionCallback); - - return !result.contacts.empty(); -} - -//============================================================================== -bool FCLCollisionDetector::detect( - CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) -{ - result.contacts.clear(); - - if (!group1 || !group2) - return false; - - if (group1->getCollisionDetector()->getType() - != FCLCollisionDetector::getTypeStatic()) - { - return false; - } - - if (group2->getCollisionDetector()->getType() - != FCLCollisionDetector::getTypeStatic()) - { - return false; - } - - group1->update(); - group2->update(); - - auto casted1 = static_cast(group1); - auto casted2 = static_cast(group2); - - auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); - auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); - - FCLCollisionCallbackData collData(this, &option, &result); - broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); - - return !result.contacts.empty(); -} - namespace { @@ -886,6 +968,8 @@ bool collisionCallback( auto collObj1 = collisionDetector->findCollisionObject(o1); auto collObj2 = collisionDetector->findCollisionObject(o2); + assert(collObj1); + assert(collObj2); if (!filter->needCollision(collObj1, collObj2)) return collData->done; @@ -906,16 +990,20 @@ bool collisionCallback( collData->done = true; } - postProcess(fclResult, o1, o2, result); + if (FCLCollisionDetector::DART == collData->mContactPointComputationMethod + && FCLCollisionDetector::MESH == collData->mPrimitiveShapeType) + postProcessDART(fclResult, o1, o2, result); + else + postProcessFCL(fclResult, o1, o2, result); return collData->done; } //============================================================================== -void postProcess(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - Result& result) +void postProcessFCL(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result) { auto numContacts = fclResult.numContacts(); @@ -991,6 +1079,249 @@ void postProcess(const fcl::CollisionResult& fclResult, } } +//============================================================================== +void postProcessDART(const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + Result& result) +{ + auto initNumContacts = fclResult.numContacts(); + + if (0 == initNumContacts) + return; + + int numCoplanarContacts = 0; + int numNoContacts = 0; + int numContacts = 0; + + std::vector unfiltered; + unfiltered.reserve(initNumContacts * 2); + + for (auto k = 0u; k < initNumContacts; ++k) + { + // for each pair of intersecting triangles, we create two contact points + Contact pair1; + Contact pair2; + const auto& c = fclResult.getContact(k); + + auto userData1 + = static_cast(o1->getUserData()); + auto userData2 + = static_cast(o2->getUserData()); + assert(userData1); + assert(userData2); + + auto collisionObject1 = userData1->mCollisionObject; + auto collisionObject2 = userData2->mCollisionObject; + + int contactResult = evalContactPosition( + c, + static_cast*>(c.o1), + static_cast*>(c.o2), + FCLTypes::convertTransform(collisionObject1->getTransform()), + FCLTypes::convertTransform(collisionObject2->getTransform()), + &pair1.point); + + if (contactResult == COPLANAR_CONTACT) + { + numCoplanarContacts++; + + if (numContacts > 2) + continue; + } + else if (contactResult == NO_CONTACT) + { + numNoContacts++; + + continue; + } + else + { + numContacts++; + } + + pair1.normal = FCLTypes::convertVector3(-c.normal); + pair1.penetrationDepth = c.penetration_depth; + pair1.triID1 = c.b1; + pair1.triID2 = c.b2; + pair1.collisionObject1 = collisionObject1; + pair1.collisionObject2 = collisionObject2; + + pair2 = pair1; + unfiltered.push_back(pair1); + unfiltered.push_back(pair2); + } + + const auto tol = 1e-12; + + auto unfilteredSize = unfiltered.size(); + + std::vector markForDeletion(unfilteredSize, false); + + // mark all the repeated points + for (auto i = 0u; i < unfilteredSize - 1u; ++i) + { + const auto& contact1 = unfiltered[i]; + + for (auto j = i + 1u; j < unfilteredSize; ++j) + { + const auto& contact2 = unfiltered[j]; + + const auto diff = contact1.point - contact2.point; + + if (diff.dot(diff) < 3.0 * tol) + { + markForDeletion[i] = true; + break; + } + } + } + + // remove all the co-linear contact points + for (auto i = 0u; i < unfilteredSize; ++i) + { + if (markForDeletion[i]) + continue; + + const auto& contact1 = unfiltered[i]; + + for (auto j = 0u; j < unfilteredSize; ++j) + { + if (i == j || markForDeletion[j]) + continue; + + if (markForDeletion[i]) + break; + + const auto& contact2 = unfiltered[j]; + + for (auto k = j + 1u; k < unfilteredSize; ++k) + { + if (i == k) + continue; + + const auto& contact3 = unfiltered[k]; + + if (isColinear(contact1.point, contact2.point, contact3.point, tol)) + { + markForDeletion[i] = true; + break; + } + } + } + } + + for (auto i = 0u; i < unfilteredSize; ++i) + { + if (!markForDeletion[i]) + result.contacts.push_back(unfiltered[i]); + } +} + +//============================================================================== +int evalContactPosition( + const fcl::Contact& fclContact, + const fcl::BVHModel* mesh1, + const fcl::BVHModel* mesh2, + const fcl::Transform3f& transform1, + const fcl::Transform3f& transform2, + Eigen::Vector3d* contactPosition) +{ + int id1 = fclContact.b1; + int id2 = fclContact.b2; + fcl::Triangle tri1 = mesh1->tri_indices[id1]; + fcl::Triangle tri2 = mesh2->tri_indices[id2]; + + fcl::Vec3f v1, v2, v3, p1, p2, p3; + v1 = mesh1->vertices[tri1[0]]; + v2 = mesh1->vertices[tri1[1]]; + v3 = mesh1->vertices[tri1[2]]; + + p1 = mesh2->vertices[tri2[0]]; + p2 = mesh2->vertices[tri2[1]]; + p3 = mesh2->vertices[tri2[2]]; + + fcl::Vec3f contact1, contact2; + v1 = transform1.transform(v1); + v2 = transform1.transform(v2); + v3 = transform1.transform(v3); + p1 = transform2.transform(p1); + p2 = transform2.transform(p2); + p3 = transform2.transform(p3); + int testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); + + if (testRes == COPLANAR_CONTACT) + { + double area1 = triArea(v1, v2, v3); + double area2 = triArea(p1, p2, p3); + + if (area1 < area2) + contact1 = v1 + v2 + v3; + else + contact1 = p1 + p2 + p3; + + contact1[0] /= 3.0; + contact1[1] /= 3.0; + contact1[2] /= 3.0; + contact2 = contact1; + } + + *contactPosition = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); + + return testRes; +} + +//============================================================================== +int FFtest( + const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, + const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, + fcl::Vec3f* res1, fcl::Vec3f* res2) +{ + float U0[3], U1[3], U2[3], V0[3], V1[3], V2[3], RES1[3], RES2[3]; + SET(U0, r1); + SET(U1, r2); + SET(U2, r3); + SET(V0, R1); + SET(V1, R2); + SET(V2, R3); + + int contactResult = tri_tri_intersect(V0, V1, V2, U0, U1, U2, RES1, RES2); + + SET((*res1), RES1); + SET((*res2), RES2); + + return contactResult; +} + +//============================================================================== +double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) +{ + fcl::Vec3f a = p2 - p1; + fcl::Vec3f b = p3 - p1; + double aMag = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]; + double bMag = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]; + double dp = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + double area = 0.5 * sqrt(aMag * bMag - dp * dp); + + return area; +} + +//============================================================================== +bool isColinear(const Eigen::Vector3d& pos1, + const Eigen::Vector3d& pos2, + const Eigen::Vector3d& pos3, + double tol) +{ + const auto va = pos1 - pos2; + const auto vb = pos1 - pos3; + const auto v = va.cross(vb); + + const auto cond1 = v.dot(v) < tol; + const auto cond2 = va.dot(vb) < 0.0; + + return cond1 && cond2; +} + //============================================================================== void convertOption(const Option& fclOption, fcl::CollisionRequest& request) { diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 2e3c7690077f4..311f66f97863c 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -73,12 +73,49 @@ class FCLCollisionDetector : public CollisionDetector std::shared_ptr createCollisionGroup( const std::vector& shapeFrames) override; - using CollisionDetector::detect; + // Documentation inherited + bool detect(CollisionGroup* group, + const Option& option, Result& result) override; + + // Documentation inherited + bool detect(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; + + /// FCL's primitive shape support is still not complete. FCL 0.4.0 fixed lots + /// of the bugs, but it still returns single contact point per shape pair + /// except for box-box collision. For this reason, we recommend using mesh for + /// primitive shapes until FCL fully support primitive shapes. + enum PrimitiveShape_t + { + MESH = 0, + PRIMITIVE + }; + + /// Set primitive shape type + void setPrimitiveShapeType(PrimitiveShape_t type); + + /// Get primitive shape type + PrimitiveShape_t getPrimitiveShapeType() const; + + /// FCL's contact computation is still (at least until 0.4.0) not correct for + /// mesh collision. We recommend using DART's contact point computation + /// instead until it's fixed in FCL. + enum ContactPointComputationMethod_t + { + DART = 0, + FCL + }; + + /// Set contact point computation method + void setContactPointComputationMethod(ContactPointComputationMethod_t method); + + /// Get contact point computation method + ContactPointComputationMethod_t getContactPointComputationMethod() const; protected: /// Constructor - FCLCollisionDetector() = default; + FCLCollisionDetector(); // Documentation inherited std::unique_ptr createCollisionObject( @@ -102,14 +139,6 @@ class FCLCollisionDetector : public CollisionDetector /// void reclaimFCLCollisionGeometry(const dynamics::ConstShapePtr& shape); - // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; - - // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; - protected: using ShapeMapValue @@ -119,6 +148,10 @@ class FCLCollisionDetector : public CollisionDetector std::map mFCLCollisionObjectMap; + PrimitiveShape_t mPrimitiveShapeType; + + ContactPointComputationMethod_t mContactPointComputationMethod; + }; } // namespace collision diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.cpp b/dart/collision/fcl/FCLMeshCollisionDetector.cpp deleted file mode 100644 index 840a8c19a2008..0000000000000 --- a/dart/collision/fcl/FCLMeshCollisionDetector.cpp +++ /dev/null @@ -1,1173 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl/FCLMeshCollisionDetector.h" - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "dart/common/Console.h" -#include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObject.h" -#include "dart/collision/fcl/tri_tri_intersection_test.h" -#include "dart/collision/fcl/FCLMeshCollisionObject.h" -#include "dart/collision/fcl/FCLMeshCollisionGroup.h" -#include "dart/dynamics/ShapeFrame.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/BoxShape.h" -#include "dart/dynamics/EllipsoidShape.h" -#include "dart/dynamics/CylinderShape.h" -#include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" -#include "dart/dynamics/MeshShape.h" -#include "dart/dynamics/SoftMeshShape.h" - -namespace dart { -namespace collision { - -namespace { - -bool collisionCallback(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata); - -void postProcess(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - Result& result); - -Contact convertContact(const fcl::Contact& fclContact, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2); - -void convertOption(const Option& option, fcl::CollisionRequest& fclRequest); - -/// Collision data stores the collision request and the result given by -/// collision algorithm. -struct FCLCollisionCallbackData -{ - /// Collision detector - FCLMeshCollisionDetector* mFclCollisionDetector; - - /// FCL collision request - fcl::CollisionRequest mFclRequest; - - /// FCL collision result - fcl::CollisionResult mFclResult; - - /// Collision option of DART - const Option* mOption; - - /// Collision result of DART - Result* mResult; - - /// Whether the collision iteration can stop - bool done; - - /// Constructor - FCLCollisionCallbackData( - FCLMeshCollisionDetector* collisionDetector, - const Option* option = nullptr, - Result* result = nullptr) - : mFclCollisionDetector(collisionDetector), - mOption(option), - mResult(result), - done(false) - { - if (mOption) - convertOption(*mOption, mFclRequest); - } -}; - -int evalContactPosition(const fcl::Contact& fclContact, - const fcl::BVHModel* mesh1, - const fcl::BVHModel* mesh2, - const fcl::Transform3f& transform1, - const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition); - -bool isColinear(const Eigen::Vector3d& pos1, - const Eigen::Vector3d& pos2, - const Eigen::Vector3d& pos3, - double tol); - -int FFtest( - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, - fcl::Vec3f* res1, fcl::Vec3f* res2); - -double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); - -//============================================================================== -// Create a cube mesh for collision detection -template -fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) -{ - int faces[6][4] = - { - {0, 1, 2, 3}, - {3, 2, 6, 7}, - {7, 6, 5, 4}, - {4, 5, 1, 0}, - {5, 6, 2, 1}, - {7, 4, 0, 3} - }; - float v[8][3]; - - v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2; - v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2; - v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2; - v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2; - v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; - v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 6; i++) - { - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); - p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - model->addTriangle(p1, p2, p3); - - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); - model->addTriangle(p1, p2, p3); - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) -{ - float v[59][3] = - { - {0, 0, 0}, - {0.135299, -0.461940, -0.135299}, - {0.000000, -0.461940, -0.191342}, - {-0.135299, -0.461940, -0.135299}, - {-0.191342, -0.461940, 0.000000}, - {-0.135299, -0.461940, 0.135299}, - {0.000000, -0.461940, 0.191342}, - {0.135299, -0.461940, 0.135299}, - {0.191342, -0.461940, 0.000000}, - {0.250000, -0.353553, -0.250000}, - {0.000000, -0.353553, -0.353553}, - {-0.250000, -0.353553, -0.250000}, - {-0.353553, -0.353553, 0.000000}, - {-0.250000, -0.353553, 0.250000}, - {0.000000, -0.353553, 0.353553}, - {0.250000, -0.353553, 0.250000}, - {0.353553, -0.353553, 0.000000}, - {0.326641, -0.191342, -0.326641}, - {0.000000, -0.191342, -0.461940}, - {-0.326641, -0.191342, -0.326641}, - {-0.461940, -0.191342, 0.000000}, - {-0.326641, -0.191342, 0.326641}, - {0.000000, -0.191342, 0.461940}, - {0.326641, -0.191342, 0.326641}, - {0.461940, -0.191342, 0.000000}, - {0.353553, 0.000000, -0.353553}, - {0.000000, 0.000000, -0.500000}, - {-0.353553, 0.000000, -0.353553}, - {-0.500000, 0.000000, 0.000000}, - {-0.353553, 0.000000, 0.353553}, - {0.000000, 0.000000, 0.500000}, - {0.353553, 0.000000, 0.353553}, - {0.500000, 0.000000, 0.000000}, - {0.326641, 0.191342, -0.326641}, - {0.000000, 0.191342, -0.461940}, - {-0.326641, 0.191342, -0.326641}, - {-0.461940, 0.191342, 0.000000}, - {-0.326641, 0.191342, 0.326641}, - {0.000000, 0.191342, 0.461940}, - {0.326641, 0.191342, 0.326641}, - {0.461940, 0.191342, 0.000000}, - {0.250000, 0.353553, -0.250000}, - {0.000000, 0.353553, -0.353553}, - {-0.250000, 0.353553, -0.250000}, - {-0.353553, 0.353553, 0.000000}, - {-0.250000, 0.353553, 0.250000}, - {0.000000, 0.353553, 0.353553}, - {0.250000, 0.353553, 0.250000}, - {0.353553, 0.353553, 0.000000}, - {0.135299, 0.461940, -0.135299}, - {0.000000, 0.461940, -0.191342}, - {-0.135299, 0.461940, -0.135299}, - {-0.191342, 0.461940, 0.000000}, - {-0.135299, 0.461940, 0.135299}, - {0.000000, 0.461940, 0.191342}, - {0.135299, 0.461940, 0.135299}, - {0.191342, 0.461940, 0.000000}, - {0.000000, -0.500000, 0.000000}, - {0.000000, 0.500000, 0.000000} - }; - - int f[112][3] = - { - {1, 2, 9}, - {9, 2, 10}, - {2, 3, 10}, - {10, 3, 11}, - {3, 4, 11}, - {11, 4, 12}, - {4, 5, 12}, - {12, 5, 13}, - {5, 6, 13}, - {13, 6, 14}, - {6, 7, 14}, - {14, 7, 15}, - {7, 8, 15}, - {15, 8, 16}, - {8, 1, 16}, - {16, 1, 9}, - {9, 10, 17}, - {17, 10, 18}, - {10, 11, 18}, - {18, 11, 19}, - {11, 12, 19}, - {19, 12, 20}, - {12, 13, 20}, - {20, 13, 21}, - {13, 14, 21}, - {21, 14, 22}, - {14, 15, 22}, - {22, 15, 23}, - {15, 16, 23}, - {23, 16, 24}, - {16, 9, 24}, - {24, 9, 17}, - {17, 18, 25}, - {25, 18, 26}, - {18, 19, 26}, - {26, 19, 27}, - {19, 20, 27}, - {27, 20, 28}, - {20, 21, 28}, - {28, 21, 29}, - {21, 22, 29}, - {29, 22, 30}, - {22, 23, 30}, - {30, 23, 31}, - {23, 24, 31}, - {31, 24, 32}, - {24, 17, 32}, - {32, 17, 25}, - {25, 26, 33}, - {33, 26, 34}, - {26, 27, 34}, - {34, 27, 35}, - {27, 28, 35}, - {35, 28, 36}, - {28, 29, 36}, - {36, 29, 37}, - {29, 30, 37}, - {37, 30, 38}, - {30, 31, 38}, - {38, 31, 39}, - {31, 32, 39}, - {39, 32, 40}, - {32, 25, 40}, - {40, 25, 33}, - {33, 34, 41}, - {41, 34, 42}, - {34, 35, 42}, - {42, 35, 43}, - {35, 36, 43}, - {43, 36, 44}, - {36, 37, 44}, - {44, 37, 45}, - {37, 38, 45}, - {45, 38, 46}, - {38, 39, 46}, - {46, 39, 47}, - {39, 40, 47}, - {47, 40, 48}, - {40, 33, 48}, - {48, 33, 41}, - {41, 42, 49}, - {49, 42, 50}, - {42, 43, 50}, - {50, 43, 51}, - {43, 44, 51}, - {51, 44, 52}, - {44, 45, 52}, - {52, 45, 53}, - {45, 46, 53}, - {53, 46, 54}, - {46, 47, 54}, - {54, 47, 55}, - {47, 48, 55}, - {55, 48, 56}, - {48, 41, 56}, - {56, 41, 49}, - {2, 1, 57}, - {3, 2, 57}, - {4, 3, 57}, - {5, 4, 57}, - {6, 5, 57}, - {7, 6, 57}, - {8, 7, 57}, - {1, 8, 57}, - {49, 50, 58}, - {50, 51, 58}, - {51, 52, 58}, - {52, 53, 58}, - {53, 54, 58}, - {54, 55, 58}, - {55, 56, 58}, - {56, 49, 58} - }; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; - model->beginModel(); - - for (int i = 0; i < 112; i++) - { - p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, - v[f[i][0]][1] * _sizeY, - v[f[i][0]][2] * _sizeZ); - p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, - v[f[i][1]][1] * _sizeY, - v[f[i][1]][2] * _sizeZ); - p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, - v[f[i][2]][1] * _sizeY, - v[f[i][2]][2] * _sizeZ); - - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - - return model; -} - -//============================================================================== -template -fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, - double _height, int _slices, int _stacks) -{ - const int CACHE_SIZE = 240; - - int i, j; - float sinCache[CACHE_SIZE]; - float cosCache[CACHE_SIZE]; - float angle; - float zBase; - float zLow, zHigh; - float sintemp, costemp; - float deltaRadius; - float radiusLow, radiusHigh; - - if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1; - - if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || - _height < 0.0) - { - return nullptr; - } - - /* Center at CoM */ - zBase = -_height/2; - - /* Compute delta */ - deltaRadius = _baseRadius - _topRadius; - - /* Cache is the vertex locations cache */ - for (i = 0; i < _slices; i++) - { - angle = 2 * M_PI * i / _slices; - sinCache[i] = sin(angle); - cosCache[i] = cos(angle); - } - - sinCache[_slices] = sinCache[0]; - cosCache[_slices] = cosCache[0]; - - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3, p4; - - model->beginModel(); - - /* Base of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _baseRadius; - zLow = zBase; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - /* Body of cylinder */ - for (i = 0; i < _slices; i++) - { - for (j = 0; j < _stacks; j++) - { - zLow = j * _height / _stacks + zBase; - zHigh = (j + 1) * _height / _stacks + zBase; - radiusLow = _baseRadius - - deltaRadius * (static_cast(j) / _stacks); - radiusHigh = _baseRadius - - deltaRadius * (static_cast(j + 1) / _stacks); - - p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], - zLow); - p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], - zLow); - p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], - zHigh); - p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], - zHigh); - - model->addTriangle(p1, p2, p3); - model->addTriangle(p2, p3, p4); - } - } - - /* Top of cylinder */ - sintemp = sinCache[0]; - costemp = cosCache[0]; - radiusLow = _topRadius; - zLow = zBase + _height; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - for (i = 1; i < _slices; i++) - { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - model->addTriangle(p1, p2, p3); - } - - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, - const aiScene* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - for (size_t i = 0; i < _mesh->mNumMeshes; i++) - { - for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) - { - fcl::Vec3f vertices[3]; - for (size_t k = 0; k < 3; k++) - { - const aiVector3D& vertex - = _mesh->mMeshes[i]->mVertices[ - _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x * _scaleX, - vertex.y * _scaleY, - vertex.z * _scaleZ); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - model->endModel(); - return model; -} - -//============================================================================== -template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) -{ - // Create FCL mesh from Assimp mesh - - assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; - model->beginModel(); - - for (size_t i = 0; i < _mesh->mNumFaces; i++) - { - fcl::Vec3f vertices[3]; - for (size_t j = 0; j < 3; j++) - { - const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - - model->endModel(); - return model; -} - -//============================================================================== -boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape) -{ - using ::dart::dynamics::Shape; - using ::dart::dynamics::BoxShape; - using ::dart::dynamics::EllipsoidShape; - using ::dart::dynamics::CylinderShape; - using ::dart::dynamics::PlaneShape; - using ::dart::dynamics::MeshShape; - using ::dart::dynamics::SoftMeshShape; - - boost::shared_ptr fclCollGeom; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - auto box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); - - auto model = new fcl::BVHModel; - fcl::generateBVHModel(*model, fcl::Box(size[0], size[1], size[2]), fcl::Transform3f()); - - fclCollGeom.reset(createCube(size[0], size[1], size[2])); -// fclCollGeom.reset(model); - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - auto ellipsoid = static_cast(shape.get()); - - const Eigen::Vector3d& size = ellipsoid->getSize(); - - fclCollGeom.reset( - createEllipsoid(size[0], size[1], size[2])); - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - auto cylinder = static_cast(shape.get()); - const double radius = cylinder->getRadius(); - const double height = cylinder->getHeight(); - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); - // TODO(JS): We still need to use mesh for cylinder since FCL 0.4.0 - // returns single contact point for cylinder yet. - break; - } - case Shape::PLANE: - { - fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - auto shapeMesh = static_cast(shape.get()); - fclCollGeom.reset(createMesh(shapeMesh->getScale()[0], - shapeMesh->getScale()[1], - shapeMesh->getScale()[2], - shapeMesh->getMesh())); - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - auto softMeshShape = static_cast(shape.get()); - fclCollGeom.reset( - createSoftMesh(softMeshShape->getAssimpMesh())); - - break; - } - default: - { - dterr << "[FCLMeshCollisionObjectData::updateShape] " - << "Attempting to create unsupported shape type '" - << shape->getShapeType() << "'.\n"; - return nullptr; - } - } - - return fclCollGeom; -} - -} // anonymous namespace - - - -//============================================================================== -std::shared_ptr FCLMeshCollisionDetector::create() -{ - return std::shared_ptr( - new FCLMeshCollisionDetector()); -} - -//============================================================================== -FCLMeshCollisionDetector::~FCLMeshCollisionDetector() -{ - assert(mShapeMap.empty()); - assert(mFCLCollisionObjectMap.empty()); -} - -//============================================================================== -const std::string& FCLMeshCollisionDetector::getType() const -{ - return getTypeStatic(); -} - -//============================================================================== -const std::string& FCLMeshCollisionDetector::getTypeStatic() -{ - static const std::string& type("FCLMesh"); - return type; -} - -//============================================================================== -std::shared_ptr FCLMeshCollisionDetector::createCollisionGroup() -{ - return std::make_shared(shared_from_this()); -} - -//============================================================================== -std::shared_ptr FCLMeshCollisionDetector::createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) -{ - return std::make_shared(shared_from_this(), - shapeFrame); -} - -//============================================================================== -std::shared_ptr FCLMeshCollisionDetector::createCollisionGroup( - const std::vector& shapeFrames) -{ - return std::make_shared(shared_from_this(), - shapeFrames); -} - -//============================================================================== -std::unique_ptr -FCLMeshCollisionDetector::createCollisionObject( - const dynamics::ShapeFrame* shapeFrame) -{ - auto fclCollGeom = claimFCLCollisionGeometry(shapeFrame->getShape()); - auto collObj = new FCLMeshCollisionObject(this, shapeFrame, fclCollGeom); - auto fclCollObj = collObj->getFCLCollisionObject(); - - mFCLCollisionObjectMap[fclCollObj] = collObj; - - return std::unique_ptr(collObj); -} - -//============================================================================== -FCLMeshCollisionObject* FCLMeshCollisionDetector::findCollisionObject( - fcl::CollisionObject* fclCollObj) const -{ - auto search = mFCLCollisionObjectMap.find(fclCollObj); - if (mFCLCollisionObjectMap.end() != search) - return search->second; - else - return nullptr; -} - -//============================================================================== -void FCLMeshCollisionDetector::notifyDestroyingCollisionObject( - CollisionObject* collObj) -{ - if (!collObj) - return; - - reclaimFCLCollisionGeometry(collObj->getShape()); - - auto casted = static_cast(collObj); - mFCLCollisionObjectMap.erase(casted->getFCLCollisionObject()); -} - -//============================================================================== -boost::shared_ptr -FCLMeshCollisionDetector::claimFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape) -{ - boost::shared_ptr fclCollGeom; - - auto search = mShapeMap.find(shape); - if (mShapeMap.end() != search) - { - auto& fclCollGeomAndCount = search->second; - - fclCollGeom = search->second.first; - - auto& count = fclCollGeomAndCount.second; - assert(0u != count); - count++; - } - else - { - fclCollGeom = createFCLCollisionGeometry(shape); - mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); - } - - return fclCollGeom; -} - -//============================================================================== -void FCLMeshCollisionDetector::reclaimFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape) -{ - auto search = mShapeMap.find(shape); - assert(mShapeMap.end() != search); - - auto& fclCollGeomAndCount = search->second; - auto& count = fclCollGeomAndCount.second; - assert(0u != count); - - count--; - - if (0u == count) - mShapeMap.erase(search); -} - -//============================================================================== -bool FCLMeshCollisionDetector::detect( - CollisionGroup* group, const Option& option, Result& result) -{ - result.contacts.clear(); - - if (!group) - return false; - - if (group->getCollisionDetector()->getType() - != FCLMeshCollisionDetector::getTypeStatic()) - { - return false; - } - - auto castedData = static_cast(group); - auto broadPhaseAlg = castedData->getFCLCollisionManager(); - - FCLCollisionCallbackData collData(this, &option, &result); - broadPhaseAlg->collide(&collData, collisionCallback); - - return !result.contacts.empty(); -} - -//============================================================================== -bool FCLMeshCollisionDetector::detect( - CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) -{ - result.contacts.clear(); - - if (!group1 || !group2) - return false; - - if (group1->getCollisionDetector()->getType() - != FCLMeshCollisionDetector::getTypeStatic()) - { - return false; - } - - if (group2->getCollisionDetector()->getType() - != FCLMeshCollisionDetector::getTypeStatic()) - { - return false; - } - - group1->update(); - group2->update(); - - auto casted1 = static_cast(group1); - auto casted2 = static_cast(group2); - - auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); - auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); - - FCLCollisionCallbackData collData(this, &option, &result); - broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); - - return !result.contacts.empty(); -} - - - -namespace { - -//============================================================================== -bool collisionCallback(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata) -{ - auto collData = static_cast(cdata); - - const auto& fclRequest = collData->mFclRequest; - auto& fclResult = collData->mFclResult; - auto& result = *(collData->mResult); - auto option = collData->mOption; - auto filter = option->collisionFilter; - - if (collData->done) - return true; - - // Filtering - if (filter) - { - auto collisionDetector = collData->mFclCollisionDetector; - - auto collObj1 = collisionDetector->findCollisionObject(o1); - auto collObj2 = collisionDetector->findCollisionObject(o2); - assert(collObj1); - assert(collObj2); - - if (!filter->needCollision(collObj1, collObj2)) - return collData->done; - } - - // Clear previous results - fclResult.clear(); - - // Perform narrow-phase detection - fcl::collide(o1, o2, fclRequest, fclResult); - - if (!fclRequest.enable_cost - && (fclResult.isCollision()) - && ((fclResult.numContacts() >= fclRequest.num_max_contacts))) - //|| !collData->checkAllCollisions)) - // TODO(JS): checkAllCollisions should be in FCLCollisionData - { - collData->done = true; - } - - postProcess(fclResult, o1, o2, result); - - return collData->done; -} - -//============================================================================== -void postProcess(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - Result& result) -{ - auto initNumContacts = fclResult.numContacts(); - - if (0 == initNumContacts) - return; - - int numCoplanarContacts = 0; - int numNoContacts = 0; - int numContacts = 0; - - std::vector unfiltered; - unfiltered.reserve(initNumContacts * 2); - - for (auto k = 0u; k < initNumContacts; ++k) - { - // for each pair of intersecting triangles, we create two contact points - Contact pair1; - Contact pair2; - const auto& c = fclResult.getContact(k); - - auto userData1 - = static_cast(o1->getUserData()); - auto userData2 - = static_cast(o2->getUserData()); - assert(userData1); - assert(userData2); - - auto collisionObject1 = userData1->mCollisionObject; - auto collisionObject2 = userData2->mCollisionObject; - - int contactResult = evalContactPosition( - c, - static_cast*>(c.o1), - static_cast*>(c.o2), - FCLTypes::convertTransform(collisionObject1->getTransform()), - FCLTypes::convertTransform(collisionObject2->getTransform()), - &pair1.point); - - if (contactResult == COPLANAR_CONTACT) - { - numCoplanarContacts++; - - if (numContacts > 2) - continue; - } - else if (contactResult == NO_CONTACT) - { - numNoContacts++; - - continue; - } - else - { - numContacts++; - } - - pair1.normal = FCLTypes::convertVector3(-c.normal); - pair1.penetrationDepth = c.penetration_depth; - pair1.triID1 = c.b1; - pair1.triID2 = c.b2; - pair1.collisionObject1 = collisionObject1; - pair1.collisionObject2 = collisionObject2; - - pair2 = pair1; - unfiltered.push_back(pair1); - unfiltered.push_back(pair2); - } - - const auto tol = 1e-12; - - auto unfilteredSize = unfiltered.size(); - - std::vector markForDeletion(unfilteredSize, false); - - // mark all the repeated points - for (auto i = 0u; i < unfilteredSize - 1u; ++i) - { - const auto& contact1 = unfiltered[i]; - - for (auto j = i + 1u; j < unfilteredSize; ++j) - { - const auto& contact2 = unfiltered[j]; - - const auto diff = contact1.point - contact2.point; - - if (diff.dot(diff) < 3.0 * tol) - { - markForDeletion[i] = true; - break; - } - } - } - - // remove all the co-linear contact points - for (auto i = 0u; i < unfilteredSize; ++i) - { - if (markForDeletion[i]) - continue; - - const auto& contact1 = unfiltered[i]; - - for (auto j = 0u; j < unfilteredSize; ++j) - { - if (i == j || markForDeletion[j]) - continue; - - if (markForDeletion[i]) - break; - - const auto& contact2 = unfiltered[j]; - - for (auto k = j + 1u; k < unfilteredSize; ++k) - { - if (i == k) - continue; - - const auto& contact3 = unfiltered[k]; - - if (isColinear(contact1.point, contact2.point, contact3.point, tol)) - { - markForDeletion[i] = true; - break; - } - } - } - } - - for (auto i = 0u; i < unfilteredSize; ++i) - { - if (!markForDeletion[i]) - result.contacts.push_back(unfiltered[i]); - } -} - -//============================================================================== -int evalContactPosition( - const fcl::Contact& fclContact, - const fcl::BVHModel* mesh1, - const fcl::BVHModel* mesh2, - const fcl::Transform3f& transform1, - const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition) -{ - int id1 = fclContact.b1; - int id2 = fclContact.b2; - fcl::Triangle tri1 = mesh1->tri_indices[id1]; - fcl::Triangle tri2 = mesh2->tri_indices[id2]; - - fcl::Vec3f v1, v2, v3, p1, p2, p3; - v1 = mesh1->vertices[tri1[0]]; - v2 = mesh1->vertices[tri1[1]]; - v3 = mesh1->vertices[tri1[2]]; - - p1 = mesh2->vertices[tri2[0]]; - p2 = mesh2->vertices[tri2[1]]; - p3 = mesh2->vertices[tri2[2]]; - - fcl::Vec3f contact1, contact2; - v1 = transform1.transform(v1); - v2 = transform1.transform(v2); - v3 = transform1.transform(v3); - p1 = transform2.transform(p1); - p2 = transform2.transform(p2); - p3 = transform2.transform(p3); - int testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); - - if (testRes == COPLANAR_CONTACT) - { - double area1 = triArea(v1, v2, v3); - double area2 = triArea(p1, p2, p3); - - if (area1 < area2) - contact1 = v1 + v2 + v3; - else - contact1 = p1 + p2 + p3; - - contact1[0] /= 3.0; - contact1[1] /= 3.0; - contact1[2] /= 3.0; - contact2 = contact1; - } - - *contactPosition = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); - - return testRes; -} - -//============================================================================== -int FFtest( - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, - fcl::Vec3f* res1, fcl::Vec3f* res2) -{ - float U0[3], U1[3], U2[3], V0[3], V1[3], V2[3], RES1[3], RES2[3]; - SET(U0, r1); - SET(U1, r2); - SET(U2, r3); - SET(V0, R1); - SET(V1, R2); - SET(V2, R3); - - int contactResult = tri_tri_intersect(V0, V1, V2, U0, U1, U2, RES1, RES2); - - SET((*res1), RES1); - SET((*res2), RES2); - - return contactResult; -} - -//============================================================================== -double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) -{ - fcl::Vec3f a = p2 - p1; - fcl::Vec3f b = p3 - p1; - double aMag = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]; - double bMag = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]; - double dp = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; - double area = 0.5 * sqrt(aMag * bMag - dp * dp); - - return area; -} - -//============================================================================== -bool isColinear(const Eigen::Vector3d& pos1, - const Eigen::Vector3d& pos2, - const Eigen::Vector3d& pos3, - double tol) -{ - const auto va = pos1 - pos2; - const auto vb = pos1 - pos3; - const auto v = va.cross(vb); - - const auto cond1 = v.dot(v) < tol; - const auto cond2 = va.dot(vb) < 0.0; - - return cond1 && cond2; -} - -//============================================================================== -void convertOption(const Option& fclOption, fcl::CollisionRequest& request) -{ - request.num_max_contacts = fclOption.maxNumContacts; - request.enable_contact = fclOption.enableContact; -#if FCL_VERSION_AT_LEAST(0,3,0) - request.gjk_solver_type = fcl::GST_LIBCCD; -#endif -} - -//============================================================================== -Contact convertContact(const fcl::Contact& fclContact, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2) -{ - Contact contact; - - Eigen::Vector3d point = FCLTypes::convertVector3(fclContact.pos); - - contact.point = point; - contact.normal = -FCLTypes::convertVector3(fclContact.normal); - contact.penetrationDepth = fclContact.penetration_depth; - contact.triID1 = fclContact.b1; - contact.triID2 = fclContact.b2; - - auto userData1 - = static_cast(o1->getUserData()); - auto userData2 - = static_cast(o2->getUserData()); - assert(userData1); - assert(userData2); - contact.collisionObject1 = userData1->mCollisionObject; - contact.collisionObject2 = userData2->mCollisionObject; - - return contact; -} - -} // anonymous namespace - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl/FCLMeshCollisionDetector.h b/dart/collision/fcl/FCLMeshCollisionDetector.h deleted file mode 100644 index f6a9ff0480943..0000000000000 --- a/dart/collision/fcl/FCLMeshCollisionDetector.h +++ /dev/null @@ -1,125 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ - -#include -#include -#include "dart/collision/CollisionDetector.h" - -namespace dart { -namespace collision { - -class FCLMeshCollisionObject; - -class FCLMeshCollisionDetector : public CollisionDetector -{ -public: - - static std::shared_ptr create(); - - /// Constructor - virtual ~FCLMeshCollisionDetector(); - - /// Return engine type "FCLMesh" - static const std::string& getTypeStatic(); - - // Documentation inherited - const std::string& getType() const override; - - // Documentation inherited - std::shared_ptr createCollisionGroup() override; - - // Documentation inherited - std::shared_ptr createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) override; - - // Documentation inherited - std::shared_ptr createCollisionGroup( - const std::vector& shapeFrames) override; - - using CollisionDetector::detect; - -protected: - - /// Constructor - FCLMeshCollisionDetector() = default; - - // Documentation inherited - std::unique_ptr createCollisionObject( - const dynamics::ShapeFrame* shapeFrame) override; - -public: - - FCLMeshCollisionObject* findCollisionObject( - fcl::CollisionObject* fclCollObj) const; - -protected: - - // Documentation inherited - void notifyDestroyingCollisionObject(CollisionObject* collObj) override; - - boost::shared_ptr claimFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape); - - // Documentation inherited - void reclaimFCLCollisionGeometry(const dynamics::ConstShapePtr& shape); - - // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; - - // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; - -protected: - - using ShapeMapValue - = std::pair, size_t>; - - std::map mShapeMap; - - std::map mFCLCollisionObjectMap; - -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONDETECTOR_H_ diff --git a/dart/collision/fcl/FCLMeshCollisionGroup.cpp b/dart/collision/fcl/FCLMeshCollisionGroup.cpp deleted file mode 100644 index c05ba5b5a2b6e..0000000000000 --- a/dart/collision/fcl/FCLMeshCollisionGroup.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl/FCLMeshCollisionGroup.h" - -#include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLMeshCollisionObject.h" - -namespace dart { -namespace collision { - -//============================================================================== -FCLMeshCollisionGroup::FCLMeshCollisionGroup( - const CollisionDetectorPtr& collisionDetector) - : CollisionGroup(collisionDetector), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) -{ - assert(mCollisionDetector); -} - -//============================================================================== -FCLMeshCollisionGroup::FCLMeshCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame) - : CollisionGroup(collisionDetector), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) -{ - assert(mCollisionDetector); - assert(shapeFrame); - - addShapeFrame(shapeFrame); -} - -//============================================================================== -FCLMeshCollisionGroup::FCLMeshCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames) - : CollisionGroup(collisionDetector), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) -{ - assert(mCollisionDetector); - - addShapeFrames(shapeFrames); -} - -//============================================================================== -FCLMeshCollisionGroup::~FCLMeshCollisionGroup() -{ - removeAllShapeFrames(); -} - -//============================================================================== -void FCLMeshCollisionGroup::initializeEngineData() -{ - mBroadPhaseAlg->setup(); -} - -//============================================================================== -void FCLMeshCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) -{ - auto casted = static_cast(object); - mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); - - this->initializeEngineData(); -} - -//============================================================================== -void FCLMeshCollisionGroup::addCollisionObjectsToEngine( - const std::vector& collObjects) -{ - for (auto collObj : collObjects) - { - auto casted = static_cast(collObj); - - mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); - } - - this->initializeEngineData(); -} - -//============================================================================== -void FCLMeshCollisionGroup::removeCollisionObjectFromEngine(CollisionObject* object) -{ - auto casted = static_cast(object); - mBroadPhaseAlg->unregisterObject(casted->getFCLCollisionObject()); - - this->initializeEngineData(); -} - -//============================================================================== -void FCLMeshCollisionGroup::removeAllCollisionObjectsFromEngine() -{ - mBroadPhaseAlg->clear(); - - this->initializeEngineData(); -} - -//============================================================================== -void FCLMeshCollisionGroup::updateEngineData() -{ - mBroadPhaseAlg->update(); -} - -//============================================================================== -FCLMeshCollisionGroup::FCLCollisionManager* -FCLMeshCollisionGroup::getFCLCollisionManager() const -{ - return mBroadPhaseAlg.get(); -} - - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl/FCLMeshCollisionGroup.h b/dart/collision/fcl/FCLMeshCollisionGroup.h deleted file mode 100644 index e41b6a411cc3e..0000000000000 --- a/dart/collision/fcl/FCLMeshCollisionGroup.h +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONGROUP_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONGROUP_H_ - -#include -#include - -#include "dart/collision/CollisionGroup.h" - -namespace dart { -namespace collision { - -class CollisionObject; -class FCLCollisionObjectUserData; - -class FCLMeshCollisionGroup : public CollisionGroup -{ -public: - - friend class FCLMeshCollisionDetector; - - using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; - using CollisionObjects = CollisionGroup::CollisionObjectPtrs; - - /// Constructor - FCLMeshCollisionGroup( - const CollisionDetectorPtr& collisionDetector); - - /// Constructor - FCLMeshCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame); - - /// Constructor - FCLMeshCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames); - - /// Destructor - virtual ~FCLMeshCollisionGroup(); - -protected: - - // Documentation inherited - void initializeEngineData() override; - - // Documentation inherited - void addCollisionObjectToEngine(CollisionObject* object) override; - - // Documentation inherited - void addCollisionObjectsToEngine( - const std::vector& collObjects) override; - - // Documentation inherited - void removeCollisionObjectFromEngine(CollisionObject* object) override; - - // Documentation inherited - void removeAllCollisionObjectsFromEngine() override; - - // Documentation inherited - void updateEngineData() override; - - /// Return FCL collision manager that is also a broad-phase algorithm - FCLCollisionManager* getFCLCollisionManager() const; - -protected: - - /// FCL broad-phase algorithm - std::unique_ptr mBroadPhaseAlg; - -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONGROUP_H_ diff --git a/dart/collision/fcl/FCLMeshCollisionObject.cpp b/dart/collision/fcl/FCLMeshCollisionObject.cpp deleted file mode 100644 index 721663d73618d..0000000000000 --- a/dart/collision/fcl/FCLMeshCollisionObject.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/collision/fcl/FCLMeshCollisionObject.h" - -#include - -#include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLTypes.h" -#include "dart/collision/fcl/FCLCollisionObject.h" -#include "dart/dynamics/ShapeFrame.h" -#include "dart/dynamics/SoftMeshShape.h" - -namespace dart { -namespace collision { - -//============================================================================== -fcl::CollisionObject* FCLMeshCollisionObject::getFCLCollisionObject() -{ - return mFCLCollisionObject.get(); -} - -//============================================================================== -const fcl::CollisionObject* FCLMeshCollisionObject::getFCLCollisionObject() const -{ - return mFCLCollisionObject.get(); -} - -//============================================================================== -FCLMeshCollisionObject::FCLMeshCollisionObject( - CollisionDetector* collisionDetector, - const dynamics::ShapeFrame* shapeFrame, - const boost::shared_ptr& fclCollGeom) - : CollisionObject(collisionDetector, shapeFrame), - mFCLCollisionObjectUserData(new FCLCollisionObject::UserData(this)), - mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) -{ - mFCLCollisionObject->setUserData(mFCLCollisionObjectUserData.get()); -} - -//============================================================================== -void FCLMeshCollisionObject::updateEngineData() -{ - using dart::dynamics::BodyNode; - using dart::dynamics::Shape; - using dart::dynamics::SoftMeshShape; - - auto collisionObject = mFCLCollisionObjectUserData->mCollisionObject; - auto shape = collisionObject->getShape().get(); - - if (shape->getShapeType() == dynamics::Shape::SOFT_MESH) - { - // Update soft-body's vertices - if (shape->getShapeType() == Shape::SOFT_MESH) - { - assert(dynamic_cast(shape)); - auto softMeshShape = static_cast(shape); - const aiMesh* mesh = softMeshShape->getAssimpMesh(); - const_cast(softMeshShape)->update(); - // TODO(JS): update function be called by somewhere out of here. - -#if FCL_VERSION_AT_LEAST(0,3,0) - auto collGeom = const_cast( - mFCLCollisionObject->collisionGeometry().get()); -#else - fcl::CollisionGeometry* collGeom - = const_cast( - mFCLCollisionObject->getCollisionGeometry()); -#endif - - assert(dynamic_cast*>(collGeom)); - auto bvhModel = static_cast*>(collGeom); - - bvhModel->beginUpdateModel(); - - for (auto j = 0u; j < mesh->mNumFaces; ++j) - { - fcl::Vec3f vertices[3]; - for (auto k = 0u; k < 3; ++k) - { - const auto& vertex = mesh->mVertices[mesh->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); - } - bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]); - } - bvhModel->endUpdateModel(); - } - } - - mFCLCollisionObject->setTransform( - FCLTypes::convertTransform(mShapeFrame->getWorldTransform())); - mFCLCollisionObject->computeAABB(); -} - -} // namespace collision -} // namespace dart diff --git a/dart/collision/fcl/FCLMeshCollisionObject.h b/dart/collision/fcl/FCLMeshCollisionObject.h deleted file mode 100644 index 40380fbe0b991..0000000000000 --- a/dart/collision/fcl/FCLMeshCollisionObject.h +++ /dev/null @@ -1,87 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECT_H_ -#define DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECT_H_ - -#include -#include - -#include - -#include "dart/collision/CollisionObject.h" -#include "dart/collision/fcl/FCLCollisionObject.h" - -namespace dart { -namespace collision { - -class FCLMeshCollisionObject : public CollisionObject -{ -public: - - friend class FCLMeshCollisionDetector; - - /// Return FCL collision object - fcl::CollisionObject* getFCLCollisionObject(); - - /// Return FCL collision object - const fcl::CollisionObject* getFCLCollisionObject() const; - -protected: - - /// Constructor - FCLMeshCollisionObject( - CollisionDetector* collisionDetector, - const dynamics::ShapeFrame* shapeFrame, - const boost::shared_ptr& fclCollGeom); - - // Documentation inherited - void updateEngineData() override; - -protected: - - /// FCL collision geometry user data - std::unique_ptr mFCLCollisionObjectUserData; - - /// FCL collision object - std::unique_ptr mFCLCollisionObject; - -}; - -} // namespace collision -} // namespace dart - -#endif // DART_COLLISION_FCL_FCLMESHCOLLISIONOBJECT_H_ diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index c7aec402af9df..b67c2b00cd3da 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -38,11 +38,7 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionGroup.h" -#if DART_USE_FCLMESHCOLLISIONDETECTOR - #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" -#else - #include "dart/collision/fcl/FCLCollisionDetector.h" -#endif +#include "dart/collision/fcl/FCLCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" @@ -68,17 +64,23 @@ using namespace dynamics; //============================================================================== ConstraintSolver::ConstraintSolver(double timeStep) -#if DART_USE_FCLMESHCOLLISIONDETECTOR - : mCollisionDetector(collision::FCLMeshCollisionDetector::create()), -#else : mCollisionDetector(collision::FCLCollisionDetector::create()), -#endif - mCollisionGroup(mCollisionDetector->createCollisionGroup()), - mCollisionOption(collision::Option(true, 100, std::make_shared())), - mTimeStep(timeStep), - mLCPSolver(new DantzigLCPSolver(mTimeStep)) + mCollisionGroup(mCollisionDetector->createCollisionGroup()), + mCollisionOption(collision::Option( + true, 100, std::make_shared())), + mTimeStep(timeStep), + mLCPSolver(new DantzigLCPSolver(mTimeStep)) { assert(timeStep > 0.0); + + auto cd = std::static_pointer_cast( + mCollisionDetector); + +#if DART_USE_FCLMESHCOLLISIONDETECTOR + cd->setPrimitiveShapeType(collision::FCLCollisionDetector::MESH); +#else + cd->setPrimitiveShapeType(collision::FCLCollisionDetector::PRIMITIVE); +#endif } //============================================================================== @@ -223,7 +225,7 @@ void ConstraintSolver::setCollisionDetector( assert(collisionDetector && "Invalid collision detector."); // Change the collision detector of the constraint solver to new one - mCollisionDetector = std::move(collisionDetector); + mCollisionDetector = collisionDetector; auto newCollisionGroup = mCollisionDetector->createCollisionGroup(); diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp index 673c3a0833cb4..14aa1cd5ee395 100644 --- a/dart/dynamics/CollisionDetector.cpp +++ b/dart/dynamics/CollisionDetector.cpp @@ -93,129 +93,5 @@ bool BodyNodeCollisionFilter::isAdjacentBodies(const BodyNode* bodyNode1, return false; } -////============================================================================== -//const Eigen::Isometry3d ShapeNodeCollisionObject::getTransform() const -//{ -// return mShapeNode->getWorldTransform(); -//} - -////============================================================================== -//bool ShapeNodeCollisionObject::isEqual( -// const collision::CollisionObject* other) const -//{ -// if (this == other) -// return true; - -// auto castedOther = dynamic_cast(other); - -// if (!castedOther) -// return false; - -// if (mShapeFrame != castedOther->mShapeFrame) -// return false; - -// if (mShapeNode != castedOther->mShapeNode) -// return false; - -// // If castedOther has the same shape and body node then it must be the same -// // pointer with this as well because the collision detector doesn't allow to -// // create new collision objects for the same shape and body node. -// assert(this != other); - -// return true; -//} - -////============================================================================== -//ShapeNode* ShapeNodeCollisionObject::getShapeNode() -//{ -// return mShapeNode.get(); -//} - -////============================================================================== -//const ShapeNode* ShapeNodeCollisionObject::getShapeNode() const -//{ -// return mShapeNode.get(); -//} - -////============================================================================== -//BodyNode* ShapeNodeCollisionObject::getBodyNode() -//{ -// return mShapeNode->getBodyNodePtr().get(); -//} - -////============================================================================== -//const BodyNode* ShapeNodeCollisionObject::getBodyNode() const -//{ -// return mShapeNode->getBodyNodePtr().get(); -//} - -////============================================================================== -//ShapeNodeCollisionObject::ShapeNodeCollisionObject( -// const collision::CollisionDetectorPtr& collisionDetector, -// const ShapePtr& shape, -// const ShapeNodePtr& shapeNode) -// : collision::CollisionObject(collisionDetector, shape), -// mShapeNode(shapeNode) -//{ -// assert(collisionDetector); -// assert(shapeNode); -// assert(shapeNode->getShape()); -// assert(shape == shapeNode->getShape()); - -// if (!mShapeNode->hasCollisionAddon()) -// { -// dtwarn << "[ShapeFrameCollisionObject::constructor] Attempting to create " -// << "ShapeFrameCollisionObject with invalid pair of Shape and " -// << "BodyNode.\n"; -// assert(false); -// } -//} - -////============================================================================== -//ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( -// const collision::CollisionDetectorPtr& collisionDetector, -// const ShapeNodePtr& shapeNode) -//{ -// return collisionDetector->getOrCreateCollisionObject( -// shapeNode->getShape(), shapeNode); -//} - -////============================================================================== -//std::vector createShapeFrameCollisionObjects( -// const collision::CollisionDetectorPtr& collisionDetector, -// const SkeletonPtr& skel) -//{ -// std::vector objects; - -// auto numBodyNodes = skel->getNumBodyNodes(); -// for (auto i = 0u; i < numBodyNodes; ++i) -// { -// auto bodyNode = skel->getBodyNode(i); -// auto collisionShapeNodes = bodyNode->getShapeNodesWith(); - -// for (auto& shapeNode : collisionShapeNodes) -// { -// auto collObj -// = createShapeNodeCollisionObject(collisionDetector, shapeNode); - -// objects.push_back( -// std::static_pointer_cast(collObj)); -// } -// } - -// return objects; -//} - -////============================================================================== -//collision::CollisionGroupPtr createShapeFrameCollisionGroup( -// const collision::CollisionDetectorPtr& collisionDetector, -// const SkeletonPtr& skel) -//{ -// auto collObjs = createShapeFrameCollisionObjects(collisionDetector, skel); -// auto group = collisionDetector->createCollisionGroup(collObjs); - -// return group; -//} - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h index 0c0efa50c597a..b4e25e439b3e2 100644 --- a/dart/dynamics/CollisionDetector.h +++ b/dart/dynamics/CollisionDetector.h @@ -57,58 +57,6 @@ struct BodyNodeCollisionFilter : collision::CollisionFilter const BodyNode* bodyNode2) const; }; -//class ShapeNodeCollisionObject : public collision::CollisionObject -//{ -//public: - -// friend class collision::CollisionDetector; - -// // Documentation inherited -// const Eigen::Isometry3d getTransform() const override; - -// // Documentation inherited -// bool isEqual(const CollisionObject* other) const override; - -// /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject -// dynamics::ShapeNode* getShapeNode(); - -// /// Return ShapeNode pointer associated with this ShapeNodeCollisionObject -// const dynamics::ShapeNode* getShapeNode() const; - -// /// Return BodyNode pointer associated with this ShapeNodeCollisionObject -// dynamics::BodyNode* getBodyNode(); - -// /// Return BodyNode pointer associated with this ShapeNodeCollisionObject -// const dynamics::BodyNode* getBodyNode() const; - -//protected: - -// ShapeNodeCollisionObject( -// const collision::CollisionDetectorPtr& collisionDetector, -// const ShapePtr& shape, -// const ShapeNodePtr& shapeNode); - -// dynamics::ShapeNodePtr mShapeNode; - -//}; - -//using ShapeNodeCollisionObjectPtr = std::shared_ptr; - -///// Create a ShapeNodeCollisionObjectPtr given ShapeNode -//ShapeNodeCollisionObjectPtr createShapeNodeCollisionObject( -// const collision::CollisionDetectorPtr& collisionDetector, -// const ShapeNodePtr& shapeNode); - -///// Create a ShapeNodeCollisionObjectPtr given Skeleton -//std::vector createShapeFrameCollisionObjects( -// const collision::CollisionDetectorPtr& collisionDetector, -// const dynamics::SkeletonPtr& skel); - -///// Create a CollisionGroup given Skeleton -//collision::CollisionGroupPtr createShapeFrameCollisionGroup( -// const collision::CollisionDetectorPtr& collisionDetector, -// const dynamics::SkeletonPtr& skel); - } // namespace dynamics } // namespace dart diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index f3f20f40373de..da3fc41b5a184 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -51,7 +51,6 @@ #endif #include "dart/collision/dart/DARTCollisionDetector.h" #include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/fcl/FCLMeshCollisionDetector.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/SoftBodyNode.h" @@ -723,9 +722,23 @@ simulation::WorldPtr readWorld( std::string strCD = getValueString(physicsElement, "collision_detector"); if (strCD == "fcl_mesh") - collision_detector = collision::FCLMeshCollisionDetector::create(); + { + collision_detector = collision::FCLCollisionDetector::create(); + auto cd = std::static_pointer_cast( + collision_detector); + cd->setPrimitiveShapeType(collision::FCLCollisionDetector::MESH); + cd->setContactPointComputationMethod( + collision::FCLCollisionDetector::DART); + } else if (strCD == "fcl") + { collision_detector = collision::FCLCollisionDetector::create(); + auto cd = std::static_pointer_cast( + collision_detector); + cd->setPrimitiveShapeType(collision::FCLCollisionDetector::PRIMITIVE); + cd->setContactPointComputationMethod( + collision::FCLCollisionDetector::DART); + } else if (strCD == "dart") collision_detector = collision::DARTCollisionDetector::create(); #ifdef HAVE_BULLET_COLLISION @@ -738,7 +751,14 @@ simulation::WorldPtr readWorld( } if (!collision_detector) - collision_detector = collision::FCLMeshCollisionDetector::create(); + { + collision_detector = collision::FCLCollisionDetector::create(); + auto cd = std::static_pointer_cast( + collision_detector); + cd->setPrimitiveShapeType(collision::FCLCollisionDetector::MESH); + cd->setContactPointComputationMethod( + collision::FCLCollisionDetector::DART); + } newWorld->getConstraintSolver()->setCollisionDetector(collision_detector); } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index f4d4bd3427dab..bba2da8988dc5 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -46,7 +46,6 @@ #include "dart/math/math.h" #include "dart/collision/CollisionGroup.h" #include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/fcl/FCLMeshCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION #include "dart/collision/bullet/BulletCollisionDetector.h" @@ -531,36 +530,44 @@ void testSimpleFrames() simpleFrame3->setShape(shape3); auto group1 = cd->createCollisionGroup(simpleFrame1.get()); -// auto group2 = cd->createCollisionGroup(simpleFrame2.get()); -// auto group3 = cd->createCollisionGroup(simpleFrame3.get()); - -// auto groupAll = cd->createCollisionGroup(); -// groupAll->unionGroup(group1); -// groupAll->unionGroup(group2); -// groupAll->unionGroup(group3); - -// collision::Option option; -// collision::Result result; - -// EXPECT_FALSE(group1->detect(option, result)); -// EXPECT_FALSE(group2->detect(option, result)); -// EXPECT_FALSE(group3->detect(option, result)); - -// simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); -// simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); -// simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); -// EXPECT_FALSE(group1->detect(group2.get(), option, result)); -// EXPECT_FALSE(group1->detect(group3.get(), option, result)); -// EXPECT_FALSE(group2->detect(group3.get(), option, result)); -// EXPECT_FALSE(groupAll->detect(option, result)); - -// simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); -// simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); -// simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); -// EXPECT_TRUE(group1->detect(group2.get(), option, result)); -// EXPECT_TRUE(group1->detect(group2.get(), option, result)); -// EXPECT_TRUE(group2->detect(group3.get(), option, result)); -// EXPECT_TRUE(groupAll->detect(option, result)); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + auto group3 = cd->createCollisionGroup(simpleFrame3.get()); + + auto groupAll = cd->createCollisionGroup(); + groupAll->unionGroup(group1); + groupAll->unionGroup(group2); + groupAll->unionGroup(group3); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + EXPECT_EQ(group3->getNumShapeFrames(), 1u); + EXPECT_EQ(groupAll->getNumShapeFrames(), + group1->getNumShapeFrames() + + group2->getNumShapeFrames() + + group3->getNumShapeFrames()); + + collision::Option option; + collision::Result result; + + EXPECT_FALSE(group1->detect(option, result)); + EXPECT_FALSE(group2->detect(option, result)); + EXPECT_FALSE(group3->detect(option, result)); + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); + EXPECT_FALSE(group1->detect(group2.get(), option, result)); + EXPECT_FALSE(group1->detect(group3.get(), option, result)); + EXPECT_FALSE(group2->detect(group3.get(), option, result)); + EXPECT_FALSE(groupAll->detect(option, result)); + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); + simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(group2->detect(group3.get(), option, result)); + EXPECT_TRUE(groupAll->detect(option, result)); } //============================================================================== @@ -568,10 +575,99 @@ TEST_F(COLLISION, SimpleFrames) { testSimpleFrames(); // testSimpleFrames(); -// testSimpleFrames(); + testSimpleFrames(); // testSimpleFrames(); } +//============================================================================== +bool checkBoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max, + const Eigen::Vector3d& point, double tol = 1e-12) +{ + for (auto i = 0u; i < 3u; ++i) + { + if (min[i] - tol > point[i] || point[i] > max[i] + tol) + return false; + } + + return true; +} + +//============================================================================== +void testBoxBox(const std::shared_ptr cd, double tol = 1e-12) +{ + auto simpleFrame1 = std::make_shared(Frame::World()); + auto simpleFrame2 = std::make_shared(Frame::World()); + + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, -0.5); + Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.5, 0.25); + simpleFrame1->setTranslation(pos1); + simpleFrame2->setTranslation(pos2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + auto groupAll = cd->createCollisionGroup(); + groupAll->unionGroup(group1); + groupAll->unionGroup(group2); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + EXPECT_EQ(groupAll->getNumShapeFrames(), + group1->getNumShapeFrames() + + group2->getNumShapeFrames()); + + collision::Option option; + collision::Result result; + + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + + Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); + Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); + + const auto numContacts = result.contacts.size(); + + EXPECT_TRUE(numContacts <= 4u); + + for (auto i = 0u; i < numContacts; ++i) + { + const auto& contact = result.contacts[i]; + const auto& point = contact.point; + + std::cout << point.transpose() << std::endl; + + EXPECT_TRUE(checkBoundingBox(min, max, point, tol)); + } +} + +//============================================================================== +TEST_F(COLLISION, BoxBox) +{ + auto fcl_prim_fcl = FCLCollisionDetector::create(); + fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + testBoxBox(fcl_prim_fcl); + + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testBoxBox(fcl_mesh_dart); + + auto fcl_mesh_fcl = FCLCollisionDetector::create(); + fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); +// testBoxBox(fcl_mesh_fcl); + + auto bullet = BulletCollisionDetector::create(); +// testBoxBox(bullet); + + auto dart = DARTCollisionDetector::create(); + testBoxBox(dart); +} + //============================================================================== template void testBodyNodes() @@ -597,44 +693,36 @@ void testBodyNodes() auto group1 = cd->createCollisionGroup(boxShapeNode1); auto group2 = cd->createCollisionGroup(boxShapeNode2); -// EXPECT_TRUE(group1->detect(group2.get(), option, result)); -// EXPECT_TRUE(cd->detect(group1.get(), group2.get(), option, result)); - -// auto group3 = cd->createCollisionGroup(boxShapeNode2); - -// EXPECT_EQ(group1->getCollisionObjects().size(), 1u); -// EXPECT_EQ(group2->getCollisionObjects().size(), 1u); - -// group1->unionGroup(group2); -// EXPECT_EQ(group1->getCollisionObjects().size(), 1u); + EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(cd->detect(group1.get(), group2.get(), option, result)); } //============================================================================== TEST_F(COLLISION, BodyNodeNodes) { testBodyNodes(); - testBodyNodes(); +// testBodyNodes(); testBodyNodes(); - testBodyNodes(); +// testBodyNodes(); } //============================================================================== template void testSkeletons() { -// auto engine = CollisionDetectorType::create(); + auto cd = CollisionDetectorType::create(); -// Eigen::Vector3d size(1.0, 1.0, 1.0); -// Eigen::Vector3d pos1(0.0, 0.0, 0.0); -// Eigen::Vector3d pos2(0.5, 0.0, 0.0); + Eigen::Vector3d size(1.0, 1.0, 1.0); + Eigen::Vector3d pos1(0.0, 0.0, 0.0); + Eigen::Vector3d pos2(0.5, 0.0, 0.0); -// auto boxSkel1 = createBox(size, pos1); -// auto boxSkel2 = createBox(size, pos2); + auto boxSkel1 = createBox(size, pos1); + auto boxSkel2 = createBox(size, pos2); -// collision::Option option; -// collision::Result result; + collision::Option option; + collision::Result result; -// auto hit = dynamics::detect(engine, boxSkel1, boxSkel2, option, result); +// auto hit = cd->detect(boxSkel1, boxSkel2, option, result); // EXPECT_TRUE(hit); } @@ -643,7 +731,7 @@ void testSkeletons() TEST_F(COLLISION, Skeletons) { testSkeletons(); - testSkeletons(); +// testSkeletons(); testSkeletons(); testSkeletons(); } From f3a89c7e57bb832aa6fc41dcbcbb22f322036c88 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 23 Mar 2016 23:04:01 -0400 Subject: [PATCH 22/67] Fix ContactConstraint -- failing only testWorld test in release mode --- dart/constraint/ContactConstraint.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index bf29588e56719..249bda834e113 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -70,21 +70,14 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, double _timeStep) : ConstraintBase(), mTimeStep(_timeStep), -// mBodyNode1(static_cast( -// _contact.collisionObject1)->getBodyNode()), -// mBodyNode2(static_cast( -// _contact.collisionObject2)->getBodyNode()), + mBodyNode1(const_cast(_contact.collisionObject1->getShapeFrame())->asShapeNode()->getBodyNodePtr().get()), + mBodyNode2(const_cast(_contact.collisionObject2->getShapeFrame())->asShapeNode()->getBodyNodePtr().get()), mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), mIsFrictionOn(true), mAppliedImpulseIndex(-1), mIsBounceOn(false), mActive(false) { -// assert(dynamic_cast( -// _contact.collisionObject1)->getBodyNode()); -// assert(dynamic_cast( -// _contact.collisionObject2)->getBodyNode()); - // TODO(JS): Assumed single contact mContacts.push_back(&_contact); From 25cac6b46eff74176f946935104e6a9bf14695e1 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 06:30:47 -0400 Subject: [PATCH 23/67] Bump version to 6.0.0 as API has changed --- CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7723623fab14c..ac50810f0acaa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,9 +43,9 @@ endif() project(dart) -set(DART_MAJOR_VERSION "5") -set(DART_MINOR_VERSION "1") -set(DART_PATCH_VERSION "1") +set(DART_MAJOR_VERSION "6") +set(DART_MINOR_VERSION "0") +set(DART_PATCH_VERSION "0") set(DART_VERSION "${DART_MAJOR_VERSION}.${DART_MINOR_VERSION}.${DART_PATCH_VERSION}") set(DART_PKG_DESC "Dynamic Animation and Robotics Toolkit.") set(DART_PKG_EXTERNAL_DEPS "flann, ccd, fcl") @@ -245,7 +245,7 @@ if(NOT BUILD_CORE_ONLY) if(WIN32 AND NOT CYGWIN) set(GLUT_INCLUDE_DIR "@CMAKE_INSTALL_PREFIX@/include") set(GLUT_LIBRARIES glut32) - else() + else()#define find_package(GLUT QUIET) if(GLUT_FOUND) message(STATUS "Looking for GLUT - found") From 5d803257cd6c8667fb35c295e57f7ceac922bfd9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 10:32:54 -0400 Subject: [PATCH 24/67] Keep special behavior of setting command depending on joint actuator type until we have actuator semantics --- dart/dynamics/SingleDofJoint.cpp | 21 --------------------- dart/dynamics/detail/MultiDofJoint.h | 21 --------------------- 2 files changed, 42 deletions(-) diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 6d159d9cafcd2..3647d65399c58 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -497,11 +497,8 @@ void SingleDofJoint::setVelocity(size_t _index, double _velocity) setVelocityStatic(_velocity); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == VELOCITY) mCommand = getVelocityStatic(); - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -527,11 +524,8 @@ void SingleDofJoint::setVelocities(const Eigen::VectorXd& _velocities) setVelocityStatic(_velocities[0]); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == VELOCITY) mCommand = getVelocityStatic(); - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -660,11 +654,8 @@ void SingleDofJoint::setAcceleration(size_t _index, double _acceleration) setAccelerationStatic(_acceleration); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == ACCELERATION) mCommand = getAccelerationStatic(); - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -690,11 +681,8 @@ void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) setAccelerationStatic(_accelerations[0]); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == ACCELERATION) mCommand = getAccelerationStatic(); - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -818,11 +806,8 @@ void SingleDofJoint::setForce(size_t _index, double _force) mForce = _force; -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == FORCE) mCommand = mForce; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -848,11 +833,8 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) mForce = _forces[0]; -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == FORCE) mCommand = mForce; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -866,11 +848,8 @@ void SingleDofJoint::resetForces() { mForce = 0.0; -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == FORCE) mCommand = mForce; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 874a9d32e1f15..5e8309a238e3f 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -620,11 +620,8 @@ void MultiDofJoint::setVelocity(size_t _index, double _velocity) mVelocities[_index] = _velocity; notifyVelocityUpdate(); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == VELOCITY) mCommands[_index] = getVelocitiesStatic()[_index]; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -652,11 +649,8 @@ void MultiDofJoint::setVelocities(const Eigen::VectorXd& _velocities) setVelocitiesStatic(_velocities); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == VELOCITY) mCommands = getVelocitiesStatic(); - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -801,11 +795,8 @@ void MultiDofJoint::setAcceleration(size_t _index, double _acceleration) mAccelerations[_index] = _acceleration; notifyAccelerationUpdate(); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == ACCELERATION) mCommands[_index] = getAccelerationsStatic()[_index]; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -833,11 +824,8 @@ void MultiDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) setAccelerationsStatic(_accelerations); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == ACCELERATION) mCommands = getAccelerationsStatic(); - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -977,11 +965,8 @@ void MultiDofJoint::setForce(size_t _index, double _force) mForces[_index] = _force; -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == FORCE) mCommands[_index] = mForces[_index]; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -1009,11 +994,8 @@ void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) mForces = _forces; -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == FORCE) mCommands = mForces; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== @@ -1029,11 +1011,8 @@ void MultiDofJoint::resetForces() { mForces.setZero(); -#if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (mJointP.mActuatorType == FORCE) mCommands = mForces; - // TODO: Remove at DART 5.1. -#endif } //============================================================================== From 161a396b5601c6f87c966760bbd4fc2e1d1fafae Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 10:35:29 -0400 Subject: [PATCH 25/67] Bump DART version of catkin package --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index d96121eb79ff9..27a17e70fa157 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ a Catkin workspace. Catkin is not required to build DART. For more information, see: http://ros.org/reps/rep-0136.html --> dart - 5.1.1 + 6.0.0 DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics From a6653d60657dcb80dcef7f756660442525355d8f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 10:43:51 -0400 Subject: [PATCH 26/67] Don't use fcl::Ellipsoid for FCL 0.3.0 or less --- dart/collision/fcl/FCLCollisionDetector.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index b3341e74272b8..71cfdea40b64b 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -614,8 +614,13 @@ boost::shared_ptr createFCLCollisionGeometry( } else { +#if FCL_VERSION_AT_LEAST(0,4,0) fclCollGeom.reset( new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); +#else + fclCollGeom.reset( + createEllipsoid(size[0], size[1], size[2])); +#endif } } else From f91a19079ecbbd0c0f46e59e4f2b2d95304b18b4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 10:59:16 -0400 Subject: [PATCH 27/67] Test bullet collision detector only when it's installed --- unittests/testCollision.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index bba2da8988dc5..96121e5fed349 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -576,7 +576,9 @@ TEST_F(COLLISION, SimpleFrames) testSimpleFrames(); // testSimpleFrames(); testSimpleFrames(); +#ifdef HAVE_BULLET_COLLISION // testSimpleFrames(); +#endif } //============================================================================== @@ -661,8 +663,10 @@ TEST_F(COLLISION, BoxBox) fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); // testBoxBox(fcl_mesh_fcl); +#ifdef HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); // testBoxBox(bullet); +#endif auto dart = DARTCollisionDetector::create(); testBoxBox(dart); @@ -703,7 +707,9 @@ TEST_F(COLLISION, BodyNodeNodes) testBodyNodes(); // testBodyNodes(); testBodyNodes(); +#ifdef HAVE_BULLET_COLLISION // testBodyNodes(); +#endif } //============================================================================== @@ -733,7 +739,9 @@ TEST_F(COLLISION, Skeletons) testSkeletons(); // testSkeletons(); testSkeletons(); +#ifdef HAVE_BULLET_COLLISION testSkeletons(); +#endif } //============================================================================== From 4bf1ec7cdb63ccd655a15da91ed312113e53d751 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 13:21:50 -0400 Subject: [PATCH 28/67] Remove mistake --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ac50810f0acaa..b90d8f674ec0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -245,7 +245,7 @@ if(NOT BUILD_CORE_ONLY) if(WIN32 AND NOT CYGWIN) set(GLUT_INCLUDE_DIR "@CMAKE_INSTALL_PREFIX@/include") set(GLUT_LIBRARIES glut32) - else()#define + else() find_package(GLUT QUIET) if(GLUT_FOUND) message(STATUS "Looking for GLUT - found") From b935da178c867442bb403a8b95d8625c7b234bce Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 25 Mar 2016 15:24:07 -0400 Subject: [PATCH 29/67] Polish CollisionGroup and CollisionObject --- dart/collision/CollisionDetector.cpp | 4 +- dart/collision/CollisionDetector.h | 6 +- dart/collision/CollisionGroup.cpp | 86 +++++++-------- dart/collision/CollisionGroup.h | 98 ++++++++++------- dart/collision/CollisionObject.cpp | 101 ------------------ dart/collision/CollisionObject.h | 73 +------------ .../bullet/BulletCollisionDetector.cpp | 12 ++- .../bullet/BulletCollisionDetector.h | 2 +- .../collision/bullet/BulletCollisionGroup.cpp | 16 +-- dart/collision/bullet/BulletCollisionGroup.h | 11 +- dart/collision/dart/DARTCollisionDetector.cpp | 2 +- dart/collision/dart/DARTCollisionDetector.h | 2 +- dart/collision/dart/DARTCollisionGroup.cpp | 14 +-- dart/collision/dart/DARTCollisionGroup.h | 8 +- .../{CollisionDetector.h => CollisionGroup.h} | 35 +++++- dart/collision/fcl/FCLCollisionDetector.cpp | 3 +- dart/collision/fcl/FCLCollisionDetector.h | 2 +- dart/collision/fcl/FCLCollisionGroup.cpp | 14 +-- dart/collision/fcl/FCLCollisionGroup.h | 9 +- dart/constraint/ConstraintSolver.cpp | 8 +- unittests/testCollision.cpp | 12 +-- unittests/testWorld.cpp | 4 +- 22 files changed, 199 insertions(+), 323 deletions(-) rename dart/collision/detail/{CollisionDetector.h => CollisionGroup.h} (63%) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index dd45d7f4e0203..3d6d64c3c3ff4 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -72,7 +72,7 @@ std::shared_ptr CollisionDetector::createCollisionGroup( = bodyNode->getShapeNodesWith(); for (auto& shapeNode : collisionShapeNodes) - group->addShapeFrame(shapeNode); + group->registerShapeFrame(shapeNode); } return group; @@ -124,7 +124,7 @@ void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) if (0u == count) { auto& collisionObject = collObjAndCount.first; - notifyDestroyingCollisionObject(collisionObject.get()); + notifyCollisionObjectDestorying(collisionObject.get()); mCollisionObjectMap.erase(search); } diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 8f0158836f8c3..d85dc7aa65691 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -92,8 +92,6 @@ class CollisionDetector : public std::enable_shared_from_this protected: - using CollisionObjectPtrs = std::vector; - /// Constructor CollisionDetector() = default; @@ -107,7 +105,7 @@ class CollisionDetector : public std::enable_shared_from_this const dynamics::ShapeFrame* shapeFrame) = 0; /// - virtual void notifyDestroyingCollisionObject(CollisionObject* collObj) = 0; + virtual void notifyCollisionObjectDestorying(CollisionObject* collObj) = 0; /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject /// will be destroyed if no CollisionGroup holds it. @@ -129,6 +127,4 @@ class CollisionDetector : public std::enable_shared_from_this } // namespace collision } // namespace dart -#include "dart/collision/detail/CollisionDetector.h" - #endif // DART_COLLISION_COLLISIONDETECTOR_H_ diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 8c3fa139fd130..571a744d61393 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -39,7 +39,6 @@ #include #include "dart/collision/CollisionObject.h" -#include "dart/collision/CollisionGroup.h" #include "dart/collision/CollisionDetector.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" @@ -68,7 +67,7 @@ CollisionDetector* CollisionGroup::getCollisionDetector() const } //============================================================================== -void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) +void CollisionGroup::registerShapeFrame(const dynamics::ShapeFrame* shapeFrame) { if (!shapeFrame) return; @@ -78,22 +77,22 @@ void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame); - addCollisionObjectToEngine(collObj); + notifyCollisionObjectAdded(collObj); mShapeFrames.push_back(shapeFrame); mCollisionObjects.push_back(collObj); } //============================================================================== -void CollisionGroup::addShapeFrames( +void CollisionGroup::registerShapeFrames( const std::vector& shapeFrames) { for (const auto& shapeFrame : shapeFrames) - addShapeFrame(shapeFrame); + registerShapeFrame(shapeFrame); } //============================================================================== -void CollisionGroup::addShapeFrames(const dynamics::Skeleton* skel) +void CollisionGroup::registerShapeFramesFrom(const dynamics::Skeleton* skel) { assert(skel); @@ -105,12 +104,18 @@ void CollisionGroup::addShapeFrames(const dynamics::Skeleton* skel) = bodyNode->getShapeNodesWith(); for (auto& shapeNode : collisionShapeNodes) - addShapeFrame(shapeNode); + registerShapeFrame(shapeNode); } } //============================================================================== -void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) +void CollisionGroup::registerShapeFramesFrom() +{ + // Do nothing +} + +//============================================================================== +void CollisionGroup::unregisterShapeFrame(const dynamics::ShapeFrame* shapeFrame) { if (!shapeFrame) return; @@ -125,7 +130,7 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) const size_t index = search - mShapeFrames.begin(); - removeCollisionObjectFromEngine(mCollisionObjects[index]); + notifyCollisionObjectRemoved(mCollisionObjects[index]); mCollisionDetector->reclaimCollisionObject(mCollisionObjects[index]); @@ -134,17 +139,40 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) } //============================================================================== -void CollisionGroup::removeShapeFrames( +void CollisionGroup::unregisterShapeFrames( const std::vector& shapeFrames) { for (const auto& shapeFrame : shapeFrames) - removeShapeFrame(shapeFrame); + unregisterShapeFrame(shapeFrame); +} + +//============================================================================== +void CollisionGroup::unregisterShapeFramesFrom(const dynamics::Skeleton* skel) +{ + assert(skel); + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + { + auto bodyNode = skel->getBodyNode(i); + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); + + for (auto& shapeNode : collisionShapeNodes) + unregisterShapeFrame(shapeNode); + } +} + +//============================================================================== +void CollisionGroup::unregisterShapeFramesFrom() +{ + // Do nothing } //============================================================================== -void CollisionGroup::removeAllShapeFrames() +void CollisionGroup::unregisterAllShapeFrames() { - removeAllCollisionObjectsFromEngine(); + notifyAllCollisionObjectsRemoved(); for (const auto& object : mCollisionObjects) mCollisionDetector->reclaimCollisionObject(object); @@ -169,38 +197,6 @@ size_t CollisionGroup::getNumShapeFrames() const return mShapeFrames.size(); } -//============================================================================== -void CollisionGroup::unionGroup(const CollisionGroupPtr& other) -{ - if (!other) - return; - - if (mCollisionDetector != other->mCollisionDetector) - { - // TODO(JS): warning message - return; - } - - for (const auto& shapeFrame : other->mShapeFrames) - addShapeFrame(shapeFrame); -} - -//============================================================================== -void CollisionGroup::subtractGroup(const CollisionGroupPtr& other) -{ - if (!other) - return; - - if (mCollisionDetector != other->mCollisionDetector) - { - // TODO(JS): warning message - return; - } - - for (const auto& shapeFrame : other->mShapeFrames) - removeShapeFrame(shapeFrame); -} - //============================================================================== void CollisionGroup::update() { diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 1c020cd6d074d..216b5b5373427 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -38,11 +38,10 @@ #define DART_COLLISION_COLLISIONGROUP_H_ #include -#include - -#include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionGroup.h" -#include "dart/collision/CollisionFilter.h" +#include "dart/collision/SmartPointer.h" +#include "dart/collision/Option.h" +#include "dart/collision/Result.h" +#include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { @@ -60,24 +59,48 @@ class CollisionGroup /// Return collision detection engine associated with this CollisionGroup CollisionDetector* getCollisionDetector() const; - /// Add a ShapeFrame to this CollisionGroup - void addShapeFrame(const dynamics::ShapeFrame* shapeFrame); + /// Register a ShapeFrame to this CollisionGroup + void registerShapeFrame(const dynamics::ShapeFrame* shapeFrame); - /// Add ShapeFrames to this CollisionGroup - void addShapeFrames( + /// Register ShapeFrames to this CollisionGroup + void registerShapeFrames( const std::vector& shapeFrames); - void addShapeFrames(const dynamics::Skeleton* skeleton); + /// Register all the ShapeFrames with CollisionAddon to this CollisionGroup + void registerShapeFramesFrom(const dynamics::Skeleton* skeleton); + + /// Register all the ShapeFrames in the other groups to this CollisionGroup + template + void registerShapeFramesFrom(const CollisionGroup* first, + const Others*... others); + + /// Do nothing. This function is for terminating the variadic template + /// function template registerShapeFramesFrom(). + void registerShapeFramesFrom(); - /// Remove collision object from this CollisionGroup - void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame); + /// Unregister a ShapeFrame from this CollisionGroup + void unregisterShapeFrame(const dynamics::ShapeFrame* shapeFrame); - /// Remove collision objects from this CollisionGroup - void removeShapeFrames( + /// Unregister ShapeFrames from this CollisionGroup + void unregisterShapeFrames( const std::vector& shapeFrames); - /// Remove all the collision object in this CollisionGroup - void removeAllShapeFrames(); + /// Unregister all the ShapeFrames with CollisionAddon from this + /// CollisionGroup + void unregisterShapeFramesFrom(const dynamics::Skeleton* skeleton); + + /// Unregister all the ShapeFrames in the other groups from this + /// CollisionGroup + template + void unregisterShapeFramesFrom(const CollisionGroup* first, + const Others*... others); + + /// Do nothing. This function is for terminating the variadic template + /// function template unregisterShapeFramesFrom(). + void unregisterShapeFramesFrom(); + + /// Unregister all the ShapeFrames in this CollisionGroup + void unregisterAllShapeFrames(); /// Return true if this CollisionGroup contains shapeFrame bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const; @@ -85,12 +108,6 @@ class CollisionGroup /// Return number of ShapeFrames added to this CollisionGroup size_t getNumShapeFrames() const; - /// Merge other CollisionGroup into this CollisionGroup - void unionGroup(const CollisionGroupPtr& other); - - /// Merge other CollisionGroup into this CollisionGroup - void subtractGroup(const CollisionGroupPtr& other); - /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void update(); @@ -104,39 +121,46 @@ class CollisionGroup /// from this CollisionObject engine. bool detect(CollisionGroup* group, const Option& option, Result& result); + /// Return all the CollisionObjects in this CollisionGroup const std::vector& getCollisionObjects(); protected: - /// Initialize the collision group data of the collision detection engine such - /// as broadphase algorithm. This function will be called whenever ShapeFrame - /// is either added to or removed from this CollisionGroup. + /// Initialize the collision detection engine data such as broadphase + /// algorithm. This function will be called whenever ShapeFrame is either + /// added to or removed from this CollisionGroup. virtual void initializeEngineData() = 0; - virtual void addCollisionObjectToEngine(CollisionObject* object) = 0; + /// Notify that a CollisionObject is added so that the collision detection + /// engine do some relevant work + virtual void notifyCollisionObjectAdded(CollisionObject* object) = 0; - virtual void addCollisionObjectsToEngine( + /// Notify that CollisionObjects are added so that the collision detection + /// engine do some relevant work + virtual void notifyCollisionObjectsAdded( const std::vector& collObjects) = 0; - virtual void removeCollisionObjectFromEngine(CollisionObject* object) = 0; + /// Notify that a CollisionObject is removed so that the collision detection + /// engine do some relevant work + virtual void notifyCollisionObjectRemoved(CollisionObject* object) = 0; - virtual void removeAllCollisionObjectsFromEngine() = 0; + /// Notify that all the CollisionObjects are remove so that the collision + /// detection engine do some relevant work + virtual void notifyAllCollisionObjectsRemoved() = 0; - /// Update the collision group of the collision detection engine such as - /// broadphase algorithm. This function will be called ahead of every - /// collision checking. + /// Update the collision detection engine data such as broadphase algorithm. + /// This function will be called ahead of every collision checking. virtual void updateEngineData() = 0; - using CollisionObjectPtr = std::shared_ptr; - using CollisionObjectPtrs = std::vector; - using ConstCollisionObjectPtrs = std::vector; - protected: /// Collision detector CollisionDetectorPtr mCollisionDetector; + /// ShapeFrames registered to this CollisionGroup std::vector mShapeFrames; + + /// CollisionObjects associated with the registered ShapeFrames std::vector mCollisionObjects; }; @@ -144,4 +168,6 @@ class CollisionGroup } // namespace collision } // namespace dart +#include "dart/collision/detail/CollisionGroup.h" + #endif // DART_COLLISION_COLLISIONGROUP_H_ diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index ea981a0ce4eb2..624be8f29af86 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -37,7 +37,6 @@ #include "dart/collision/CollisionObject.h" #include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionObject.h" #include "dart/dynamics/ShapeFrame.h" namespace dart { @@ -61,24 +60,12 @@ const CollisionDetector* CollisionObject::getCollisionDetector() const return mCollisionDetector; } -////============================================================================== -//const dynamics::ShapeFrame* CollisionObject::getShapeFrame() -//{ -// return mShapeFrame; -//} - //============================================================================== const dynamics::ShapeFrame* CollisionObject::getShapeFrame() const { return mShapeFrame; } -////============================================================================== -//dynamics::ShapePtr CollisionObject::getShape() -//{ -// return mShapeFrame->getShape(); -//} - //============================================================================== dynamics::ConstShapePtr CollisionObject::getShape() const { @@ -91,21 +78,6 @@ const Eigen::Isometry3d& CollisionObject::getTransform() const return mShapeFrame->getWorldTransform(); } -////============================================================================== -//bool CollisionObject::detect(CollisionObject* other, -// const Option& option, -// Result& result) -//{ -// return mCollisionDetector->detect(this, other, option, result); -//} - -////============================================================================== -//bool CollisionObject::detect(CollisionGroup* group, -// const Option& option, Result& result) -//{ -// return mCollisionDetector->detect(this, group, option, result); -//} - //============================================================================== CollisionObject::CollisionObject( CollisionDetector* collisionDetector, @@ -117,78 +89,5 @@ CollisionObject::CollisionObject( assert(mShapeFrame); } -//============================================================================== -void CollisionObject::addGroup(CollisionGroup* group) -{ - if (!group) - return; - - if (!hasGroup(group)) - mGroups.push_back(group); -} - -//============================================================================== -void CollisionObject::removeGroup(CollisionGroup* group) -{ - mGroups.erase(std::remove(mGroups.begin(), mGroups.end(), group), - mGroups.end()); -} - -//============================================================================== -bool CollisionObject::hasGroup(CollisionGroup* group) -{ - return std::find(mGroups.begin(), mGroups.end(), group) != mGroups.end(); -} - -////============================================================================== -//std::shared_ptr FreeCollisionObject::create( -// const CollisionDetectorPtr& collisionDetector, -// const dynamics::ShapePtr& shape, const Eigen::Isometry3d& tf) -//{ -// return collisionDetector->createCollisionObject( -// shape, tf); -//} - -////============================================================================== -//void FreeCollisionObject::setTransform(const Eigen::Isometry3d& tf) -//{ -// mW = tf; -//} - -////============================================================================== -//void FreeCollisionObject::setRotation(const Eigen::Matrix3d& rotation) -//{ -// mW.linear() = rotation; -//} - -////============================================================================== -//void FreeCollisionObject::setTranslation(const Eigen::Vector3d& translation) -//{ -// mW.translation() = translation; -//} - -////============================================================================== -//const Eigen::Isometry3d FreeCollisionObject::getTransform() const -//{ -// return mW; -//} - -////============================================================================== -//bool FreeCollisionObject::isEqual(const CollisionObject* other) const -//{ -// return this == other; -//} - -////============================================================================== -//FreeCollisionObject::FreeCollisionObject( -// const CollisionDetectorPtr& collisionDetector, -// const dynamics::ShapePtr& shape, -// const Eigen::Isometry3d& tf) -// : CollisionObject(collisionDetector, shape), -// mW(tf) -//{ -// // Do nothing -//} - } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index 1b7be5bbe35ea..cf9c8cc05594a 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -39,21 +39,19 @@ #include -#include "dart/collision/CollisionDetector.h" #include "dart/collision/SmartPointer.h" -#include "dart/collision/CollisionObject.h" #include "dart/dynamics/SmartPointer.h" namespace dart { namespace collision { -// TODO: this should be in collision::detail namespace class CollisionObject { public: friend class CollisionGroup; + /// Destructor virtual ~CollisionObject(); /// Return collision detection engine associated with this CollisionObject @@ -62,33 +60,15 @@ class CollisionObject /// Return collision detection engine associated with this CollisionObject const CollisionDetector* getCollisionDetector() const; -// /// Return the associated ShapeFrame -// dynamics::ShapeFrame* getShapeFrame(); - /// Return the associated ShapeFrame const dynamics::ShapeFrame* getShapeFrame() const; -// /// Return the associated Shape -// dynamics::ShapePtr getShape(); - /// Return the associated Shape dynamics::ConstShapePtr getShape() const; /// Return the transformation of this CollisionObject in world coordinates const Eigen::Isometry3d& getTransform() const; - /// Perform collision detection with other CollisionObject. - /// - /// Return false if the engine type of the other CollisionObject is different - /// from this CollisionObject engine. -// bool detect(CollisionObject* other, const Option& option, Result& result); - - /// Perform collision detection with other CollisionGroup. - /// - /// Return false if the engine type of the other CollisionGroup is different - /// from this CollisionObject engine. -// bool detect(CollisionGroup* group, const Option& option, Result& result); - protected: /// Contructor @@ -99,13 +79,6 @@ class CollisionObject /// function will be called ahead of every collision checking by /// CollisionGroup. virtual void updateEngineData() = 0; - // TODO(JS): Rename - - void addGroup(CollisionGroup* group); - - void removeGroup(CollisionGroup* group); - - bool hasGroup(CollisionGroup* group); protected: @@ -115,52 +88,8 @@ class CollisionObject /// ShapeFrame const dynamics::ShapeFrame* mShapeFrame; - std::vector mGroups; - }; -/// FreeCollisionOjbect is a basic collision object whose transformation can be -/// set freely. -//class FreeCollisionObject : public CollisionObject -//{ -//public: - -// friend class CollisionDetector; - -// static std::shared_ptr create( -// const CollisionDetectorPtr& collisionDetector, -// const dynamics::ShapePtr& shape, -// const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); - -// /// Set world transformation of this FreeCollisionObject -// void setTransform(const Eigen::Isometry3d& tf); - -// /// Set world rotation of this FreeCollisionObject -// void setRotation(const Eigen::Matrix3d& rotation); - -// /// Set world translation of this FreeCollisionObject -// void setTranslation(const Eigen::Vector3d& translation); - -// /// Return world transformation of this FreeCollisionObject -// const Eigen::Isometry3d getTransform() const override; - -// // Documentation inherited -// bool isEqual(const CollisionObject* other) const override; - -//protected: - -// /// Constructor -// FreeCollisionObject( -// const CollisionDetectorPtr& collisionDetector, -// const dynamics::ShapePtr& shape, -// const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()); -// // TODO(JS): change to engine pointer - -// /// Transformation in world coordinates -// Eigen::Isometry3d mW; - -//}; - } // namespace collision } // namespace dart diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 06b2fdf9dabc5..3a5ee918c41f4 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -40,6 +40,7 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionFilter.h" #include "dart/collision/bullet/BulletTypes.h" #include "dart/collision/bullet/BulletCollisionObject.h" #include "dart/collision/bullet/BulletCollisionGroup.h" @@ -212,7 +213,7 @@ bool BulletCollisionDetector::detect( //============================================================================== bool BulletCollisionDetector::detect( CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) + const Option& /*option*/, Result& result) { result.contacts.clear(); @@ -222,13 +223,13 @@ bool BulletCollisionDetector::detect( if (group1->getCollisionDetector()->getType() != BulletCollisionDetector::getTypeStatic()) { - return false; + return false; } if (group2->getCollisionDetector()->getType() != BulletCollisionDetector::getTypeStatic()) { - return false; + return false; } auto castedData1 = static_cast(group1); @@ -237,6 +238,9 @@ bool BulletCollisionDetector::detect( auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); + mBulletCollisionGroupForSinglePair->unregisterAllShapeFrames(); + mBulletCollisionGroupForSinglePair->registerShapeFramesFrom(group1, group2); + // BulletCollisionData collData(&option, &result); // bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); // TODO(JS) @@ -271,7 +275,7 @@ BulletCollisionObject* BulletCollisionDetector::findCollisionObject( } //============================================================================== -void BulletCollisionDetector::notifyDestroyingCollisionObject( +void BulletCollisionDetector::notifyCollisionObjectDestorying( CollisionObject* collObj) { if (!collObj) diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 4677503f599f4..8d112ee25056a 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -107,7 +107,7 @@ class BulletCollisionDetector : public CollisionDetector protected: // Documentation inherited - void notifyDestroyingCollisionObject(CollisionObject* collObj) override; + void notifyCollisionObjectDestorying(CollisionObject* collObj) override; /// BulletCollsionPack claimBulletCollisionGeometry( diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index f1489a2511f81..8948f157609fe 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -79,7 +79,7 @@ BulletCollisionGroup::BulletCollisionGroup( assert(mCollisionDetector); assert(shapeFrame); - addShapeFrame(shapeFrame); + registerShapeFrame(shapeFrame); } //============================================================================== @@ -100,13 +100,13 @@ BulletCollisionGroup::BulletCollisionGroup( { assert(mCollisionDetector); - addShapeFrames(shapeFrames); + registerShapeFrames(shapeFrames); } //============================================================================== BulletCollisionGroup::~BulletCollisionGroup() { - removeAllShapeFrames(); + unregisterAllShapeFrames(); } //============================================================================== @@ -118,7 +118,7 @@ void BulletCollisionGroup::initializeEngineData() } //============================================================================== -void BulletCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) +void BulletCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) { auto casted = static_cast(object); @@ -128,7 +128,7 @@ void BulletCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) } //============================================================================== -void BulletCollisionGroup::addCollisionObjectsToEngine( +void BulletCollisionGroup::notifyCollisionObjectsAdded( const std::vector& collObjects) { for (auto collObj : collObjects) @@ -143,7 +143,7 @@ void BulletCollisionGroup::addCollisionObjectsToEngine( } //============================================================================== -void BulletCollisionGroup::removeCollisionObjectFromEngine( +void BulletCollisionGroup::notifyCollisionObjectRemoved( CollisionObject* object) { auto casted = static_cast(object); @@ -155,10 +155,10 @@ void BulletCollisionGroup::removeCollisionObjectFromEngine( } //============================================================================== -void BulletCollisionGroup::removeAllCollisionObjectsFromEngine() +void BulletCollisionGroup::notifyAllCollisionObjectsRemoved() { for (const auto& collisionObject : getCollisionObjects()) - removeCollisionObjectFromEngine(collisionObject); + notifyCollisionObjectRemoved(collisionObject); this->initializeEngineData(); } diff --git a/dart/collision/bullet/BulletCollisionGroup.h b/dart/collision/bullet/BulletCollisionGroup.h index 92fe69666d476..981c2ff26f151 100644 --- a/dart/collision/bullet/BulletCollisionGroup.h +++ b/dart/collision/bullet/BulletCollisionGroup.h @@ -50,8 +50,6 @@ class BulletCollisionGroup : public CollisionGroup friend class BulletCollisionDetector; - using CollisionObjects = CollisionGroup::CollisionObjectPtrs; - /// Constructor BulletCollisionGroup( const CollisionDetectorPtr& collisionDetector); @@ -75,17 +73,17 @@ class BulletCollisionGroup : public CollisionGroup void initializeEngineData() override; // Documentation inherited - void addCollisionObjectToEngine(CollisionObject* object) override; + void notifyCollisionObjectAdded(CollisionObject* object) override; // Documentation inherited - void addCollisionObjectsToEngine( + void notifyCollisionObjectsAdded( const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObjectFromEngine(CollisionObject* object) override; + void notifyCollisionObjectRemoved(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjectsFromEngine() override; + void notifyAllCollisionObjectsRemoved() override; // Documentation inherited void updateEngineData() override; @@ -106,6 +104,7 @@ class BulletCollisionGroup : public CollisionGroup /// Bullet collision world std::unique_ptr mBulletCollisionWorld; + }; } // namespace collision diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 27fb5b7aad0f2..f3accf1d3df86 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -178,7 +178,7 @@ std::unique_ptr DARTCollisionDetector::createCollisionObject( } //============================================================================== -void DARTCollisionDetector::notifyDestroyingCollisionObject( +void DARTCollisionDetector::notifyCollisionObjectDestorying( CollisionObject* collObj) { if (!collObj) diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 7e2f5e0d6c527..7b64fe0a690f1 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -86,7 +86,7 @@ class DARTCollisionDetector : public CollisionDetector const dynamics::ShapeFrame* shapeFrame) override; // Documentation inherited - void notifyDestroyingCollisionObject(CollisionObject* collObj) override; + void notifyCollisionObjectDestorying(CollisionObject* collObj) override; protected: diff --git a/dart/collision/dart/DARTCollisionGroup.cpp b/dart/collision/dart/DARTCollisionGroup.cpp index 17edf49af4362..22565f80afb42 100644 --- a/dart/collision/dart/DARTCollisionGroup.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -58,7 +58,7 @@ DARTCollisionGroup::DARTCollisionGroup( assert(mCollisionDetector); assert(shapeFrame); - addShapeFrame(shapeFrame); + registerShapeFrame(shapeFrame); } //============================================================================== @@ -69,13 +69,13 @@ DARTCollisionGroup::DARTCollisionGroup( { assert(mCollisionDetector); - addShapeFrames(shapeFrames); + registerShapeFrames(shapeFrames); } //============================================================================== DARTCollisionGroup::~DARTCollisionGroup() { - removeAllShapeFrames(); + unregisterAllShapeFrames(); } //============================================================================== @@ -85,27 +85,27 @@ void DARTCollisionGroup::initializeEngineData() } //============================================================================== -void DARTCollisionGroup::addCollisionObjectToEngine(CollisionObject* /*object*/) +void DARTCollisionGroup::notifyCollisionObjectAdded(CollisionObject* /*object*/) { // Do nothing } //============================================================================== -void DARTCollisionGroup::addCollisionObjectsToEngine( +void DARTCollisionGroup::notifyCollisionObjectsAdded( const std::vector& /*collObjects*/) { // Do nothing } //============================================================================== -void DARTCollisionGroup::removeCollisionObjectFromEngine( +void DARTCollisionGroup::notifyCollisionObjectRemoved( CollisionObject* /*object*/) { // Do nothing } //============================================================================== -void DARTCollisionGroup::removeAllCollisionObjectsFromEngine() +void DARTCollisionGroup::notifyAllCollisionObjectsRemoved() { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionGroup.h b/dart/collision/dart/DARTCollisionGroup.h index 62b0840a84acc..27018570345cc 100644 --- a/dart/collision/dart/DARTCollisionGroup.h +++ b/dart/collision/dart/DARTCollisionGroup.h @@ -69,17 +69,17 @@ class DARTCollisionGroup : public CollisionGroup void initializeEngineData() override; // Documentation inherited - void addCollisionObjectToEngine(CollisionObject* object) override; + void notifyCollisionObjectAdded(CollisionObject* object) override; // Documentation inherited - void addCollisionObjectsToEngine( + void notifyCollisionObjectsAdded( const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObjectFromEngine(CollisionObject* object) override; + void notifyCollisionObjectRemoved(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjectsFromEngine() override; + void notifyAllCollisionObjectsRemoved() override; // Documentation inherited void updateEngineData() override; diff --git a/dart/collision/detail/CollisionDetector.h b/dart/collision/detail/CollisionGroup.h similarity index 63% rename from dart/collision/detail/CollisionDetector.h rename to dart/collision/detail/CollisionGroup.h index fee63aaa696ce..4c8f05bdbc950 100644 --- a/dart/collision/detail/CollisionDetector.h +++ b/dart/collision/detail/CollisionGroup.h @@ -34,17 +34,44 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ -#define DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ +#ifndef DART_COLLISION_DETAIL_COLLISIONGROUP_H_ +#define DART_COLLISION_DETAIL_COLLISIONGROUP_H_ -#include "dart/collision/CollisionDetector.h" +#include "dart/collision/CollisionGroup.h" +#include "dart/common/Console.h" namespace dart { namespace collision { +//============================================================================== +template +void CollisionGroup::registerShapeFramesFrom(const CollisionGroup* first, + const Others*... others) +{ + if (!first) + return; + for (const auto& shapeFrame : first->mShapeFrames) + registerShapeFrame(shapeFrame); + + registerShapeFramesFrom(others...); +} + +//============================================================================== +template +void CollisionGroup::unregisterShapeFramesFrom(const CollisionGroup* first, + const Others*... others) +{ + if (!first) + return; + + for (const auto& shapeFrame : first->mShapeFrames) + unregisterShapeFrame(shapeFrame); + + registerShapeFramesFrom(others...); +} } // namespace collision } // namespace dart -#endif // DART_COLLISION_COLLISIONDETECTOR_H_ +#endif // DART_COLLISION_DETAIL_COLLISIONGROUP_H_ diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 71cfdea40b64b..2322094be794f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -49,6 +49,7 @@ #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionFilter.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/collision/fcl/FCLCollisionObject.h" #include "dart/collision/fcl/FCLCollisionGroup.h" @@ -891,7 +892,7 @@ FCLCollisionObject* FCLCollisionDetector::findCollisionObject( } //============================================================================== -void FCLCollisionDetector::notifyDestroyingCollisionObject( +void FCLCollisionDetector::notifyCollisionObjectDestorying( CollisionObject* collObj) { if (!collObj) diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 311f66f97863c..63772c5cb603f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -130,7 +130,7 @@ class FCLCollisionDetector : public CollisionDetector protected: // Documentation inherited - void notifyDestroyingCollisionObject(CollisionObject* collObj) override; + void notifyCollisionObjectDestorying(CollisionObject* collObj) override; /// boost::shared_ptr claimFCLCollisionGeometry( diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index 0fe0ac62a0560..d80fe3cca5f41 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -61,7 +61,7 @@ FCLCollisionGroup::FCLCollisionGroup( assert(mCollisionDetector); assert(shapeFrame); - addShapeFrame(shapeFrame); + registerShapeFrame(shapeFrame); } //============================================================================== @@ -73,13 +73,13 @@ FCLCollisionGroup::FCLCollisionGroup( { assert(mCollisionDetector); - addShapeFrames(shapeFrames); + registerShapeFrames(shapeFrames); } //============================================================================== FCLCollisionGroup::~FCLCollisionGroup() { - removeAllShapeFrames(); + unregisterAllShapeFrames(); } //============================================================================== @@ -89,7 +89,7 @@ void FCLCollisionGroup::initializeEngineData() } //============================================================================== -void FCLCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) +void FCLCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) { auto casted = static_cast(object); mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); @@ -98,7 +98,7 @@ void FCLCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) } //============================================================================== -void FCLCollisionGroup::addCollisionObjectsToEngine( +void FCLCollisionGroup::notifyCollisionObjectsAdded( const std::vector& collObjects) { for (auto collObj : collObjects) @@ -112,7 +112,7 @@ void FCLCollisionGroup::addCollisionObjectsToEngine( } //============================================================================== -void FCLCollisionGroup::removeCollisionObjectFromEngine(CollisionObject* object) +void FCLCollisionGroup::notifyCollisionObjectRemoved(CollisionObject* object) { auto casted = static_cast(object); @@ -122,7 +122,7 @@ void FCLCollisionGroup::removeCollisionObjectFromEngine(CollisionObject* object) } //============================================================================== -void FCLCollisionGroup::removeAllCollisionObjectsFromEngine() +void FCLCollisionGroup::notifyAllCollisionObjectsRemoved() { mBroadPhaseAlg->clear(); diff --git a/dart/collision/fcl/FCLCollisionGroup.h b/dart/collision/fcl/FCLCollisionGroup.h index c29a6adc42098..3a5b848f37daf 100644 --- a/dart/collision/fcl/FCLCollisionGroup.h +++ b/dart/collision/fcl/FCLCollisionGroup.h @@ -54,7 +54,6 @@ class FCLCollisionGroup : public CollisionGroup friend class FCLCollisionDetector; using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; - using CollisionObjects = CollisionGroup::CollisionObjectPtrs; /// Constructor FCLCollisionGroup( @@ -79,17 +78,17 @@ class FCLCollisionGroup : public CollisionGroup void initializeEngineData() override; // Documentation inherited - void addCollisionObjectToEngine(CollisionObject* object) override; + void notifyCollisionObjectAdded(CollisionObject* object) override; // Documentation inherited - void addCollisionObjectsToEngine( + void notifyCollisionObjectsAdded( const std::vector& collObjects) override; // Documentation inherited - void removeCollisionObjectFromEngine(CollisionObject* object) override; + void notifyCollisionObjectRemoved(CollisionObject* object) override; // Documentation inherited - void removeAllCollisionObjectsFromEngine() override; + void notifyAllCollisionObjectsRemoved() override; // Documentation inherited void updateEngineData() override; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index b67c2b00cd3da..80bca5f32e1fd 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -98,7 +98,7 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) if (!containSkeleton(skeleton)) { auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->unionGroup(group); + mCollisionGroup->registerShapeFramesFrom(group.get()); mSkeletons.push_back(skeleton); mConstrainedGroups.reserve(mSkeletons.size()); @@ -127,7 +127,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) if (containSkeleton(skeleton)) { auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->subtractGroup(group); + mCollisionGroup->unregisterShapeFramesFrom(group.get()); mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), mSkeletons.end()); @@ -152,7 +152,7 @@ void ConstraintSolver::removeSkeletons( //============================================================================== void ConstraintSolver::removeAllSkeletons() { - mCollisionGroup->removeAllShapeFrames(); + mCollisionGroup->unregisterAllShapeFrames(); mSkeletons.clear(); } @@ -230,7 +230,7 @@ void ConstraintSolver::setCollisionDetector( auto newCollisionGroup = mCollisionDetector->createCollisionGroup(); for (const auto& skeleton : mSkeletons) - newCollisionGroup->addShapeFrames(skeleton.get()); + newCollisionGroup->registerShapeFramesFrom(skeleton.get()); mCollisionGroup = newCollisionGroup; } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 96121e5fed349..33d9188b16bfd 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -534,9 +534,9 @@ void testSimpleFrames() auto group3 = cd->createCollisionGroup(simpleFrame3.get()); auto groupAll = cd->createCollisionGroup(); - groupAll->unionGroup(group1); - groupAll->unionGroup(group2); - groupAll->unionGroup(group3); + groupAll->registerShapeFramesFrom(group1.get()); + groupAll->registerShapeFramesFrom(group2.get()); + groupAll->registerShapeFramesFrom(group3.get()); EXPECT_EQ(group1->getNumShapeFrames(), 1u); EXPECT_EQ(group2->getNumShapeFrames(), 1u); @@ -613,8 +613,8 @@ void testBoxBox(const std::shared_ptr cd, double tol = 1e-12) auto group1 = cd->createCollisionGroup(simpleFrame1.get()); auto group2 = cd->createCollisionGroup(simpleFrame2.get()); auto groupAll = cd->createCollisionGroup(); - groupAll->unionGroup(group1); - groupAll->unionGroup(group2); + groupAll->registerShapeFramesFrom(group1.get()); + groupAll->registerShapeFramesFrom(group2.get()); EXPECT_EQ(group1->getNumShapeFrames(), 1u); EXPECT_EQ(group2->getNumShapeFrames(), 1u); @@ -665,7 +665,7 @@ TEST_F(COLLISION, BoxBox) #ifdef HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); -// testBoxBox(bullet); + testBoxBox(bullet); #endif auto dart = DARTCollisionDetector::create(); diff --git a/unittests/testWorld.cpp b/unittests/testWorld.cpp index 3d0cbc6987976..31fe25418c3ce 100644 --- a/unittests/testWorld.cpp +++ b/unittests/testWorld.cpp @@ -262,6 +262,6 @@ TEST(World, Cloning) //============================================================================== int main(int argc, char* argv[]) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } From 0be239c756b8c702f5b88df74176ed4752c899f0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 11:34:19 -0400 Subject: [PATCH 30/67] BulletCollisionDetector works for group-group --- dart/collision/CollisionDetector.cpp | 103 ++++++++++++++++-- dart/collision/CollisionDetector.h | 87 ++++++++++++++- .../bullet/BulletCollisionDetector.cpp | 27 +++-- .../bullet/BulletCollisionDetector.h | 5 +- .../collision/bullet/BulletCollisionGroup.cpp | 20 ++-- .../bullet/BulletCollisionObject.cpp | 2 +- dart/collision/dart/DARTCollisionDetector.cpp | 6 + dart/collision/dart/DARTCollisionDetector.h | 2 +- dart/collision/detail/CollisionGroup.h | 24 ++-- dart/collision/fcl/FCLCollisionDetector.cpp | 5 +- dart/collision/fcl/FCLCollisionGroup.cpp | 8 +- unittests/testCollision.cpp | 23 ++-- 12 files changed, 237 insertions(+), 75 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 3d6d64c3c3ff4..de6baccc6ef32 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -50,12 +50,6 @@ namespace dart { namespace collision { -//============================================================================== -CollisionDetector::~CollisionDetector() -{ - assert(mCollisionObjectMap.empty()); -} - //============================================================================== std::shared_ptr CollisionDetector::createCollisionGroup( dynamics::Skeleton* skel) @@ -81,6 +75,92 @@ std::shared_ptr CollisionDetector::createCollisionGroup( //============================================================================== CollisionObject* CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) +{ + if (!mCollisionObjectManager) + mCollisionObjectManager.reset(new NaiveCollisionObjectManager(this)); + + return mCollisionObjectManager->claimCollisionObject(shapeFrame); +} + +//============================================================================== +void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) +{ + if (!mCollisionObjectManager) + mCollisionObjectManager.reset(new NaiveCollisionObjectManager(this)); + + mCollisionObjectManager->reclaimCollisionObject(collObj); +} + +//============================================================================== +CollisionDetector::CollisionObjectManager::CollisionObjectManager( + CollisionDetector* cd) + : mCollisionDetector(cd) +{ + // Do nothing +} + +//============================================================================== +CollisionDetector:: +NaiveCollisionObjectManager::NaiveCollisionObjectManager( + CollisionDetector* cd) + : CollisionDetector::CollisionObjectManager(cd) +{ + // Do nothing +} + +//============================================================================== +CollisionDetector:: +NaiveCollisionObjectManager::~NaiveCollisionObjectManager() +{ + assert(mCollisionObjects.empty()); +} + +//============================================================================== +CollisionObject* +CollisionDetector::NaiveCollisionObjectManager::claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) +{ + mCollisionObjects.push_back( + std::move(mCollisionDetector->createCollisionObject(shapeFrame))); + + return mCollisionObjects.back().get(); +} + +//============================================================================== +void CollisionDetector::NaiveCollisionObjectManager::reclaimCollisionObject( + const CollisionObject* object) +{ + auto search = std::find_if(mCollisionObjects.begin(), mCollisionObjects.end(), + [&](const std::unique_ptr& it) + { return it.get() == object; }); + + if (mCollisionObjects.end() == search) + return; + + mCollisionDetector->notifyCollisionObjectDestorying(search->get()); + mCollisionObjects.erase(search); +} + +//============================================================================== +CollisionDetector:: +RefCountingCollisionObjectManager::RefCountingCollisionObjectManager( + CollisionDetector* cd) + : CollisionDetector::CollisionObjectManager(cd) +{ + // Do nothing +} + +//============================================================================== +CollisionDetector:: +RefCountingCollisionObjectManager::~RefCountingCollisionObjectManager() +{ + assert(mCollisionObjectMap.empty()); +} + +//============================================================================== +CollisionObject* +CollisionDetector::RefCountingCollisionObjectManager::claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { auto search = mCollisionObjectMap.find(shapeFrame); @@ -98,7 +178,8 @@ CollisionObject* CollisionDetector::claimCollisionObject( return collObj.get(); } - auto newCollisionObject = createCollisionObject(shapeFrame); + auto newCollisionObject + = mCollisionDetector->createCollisionObject(shapeFrame); mCollisionObjectMap[shapeFrame] = std::make_pair(std::move(newCollisionObject), 1u); @@ -107,9 +188,11 @@ CollisionObject* CollisionDetector::claimCollisionObject( } //============================================================================== -void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) +void CollisionDetector:: +RefCountingCollisionObjectManager::reclaimCollisionObject( + const CollisionObject* object) { - auto shapeFrame = collObj->getShapeFrame(); + auto shapeFrame = object->getShapeFrame(); auto search = mCollisionObjectMap.find(shapeFrame); if (mCollisionObjectMap.end() == search) @@ -124,7 +207,7 @@ void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) if (0u == count) { auto& collisionObject = collObjAndCount.first; - notifyCollisionObjectDestorying(collisionObject.get()); + mCollisionDetector->notifyCollisionObjectDestorying(collisionObject.get()); mCollisionObjectMap.erase(search); } diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index d85dc7aa65691..691b19bbf80ea 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -61,9 +61,6 @@ class CollisionDetector : public std::enable_shared_from_this friend class CollisionObject; friend class CollisionGroup; - /// Destructor - virtual ~CollisionDetector(); - /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; @@ -92,24 +89,102 @@ class CollisionDetector : public std::enable_shared_from_this protected: + // We define following collision object managers outside of this class for + // better code readability. + class CollisionObjectManager; + class NaiveCollisionObjectManager; + class RefCountingCollisionObjectManager; + /// Constructor CollisionDetector() = default; - /// Return CollisionObject associated with shapeFrame. New CollisionObject + /// Claim CollisionObject associated with shapeFrame. New CollisionObject /// will be created if it hasn't created yet for shapeFrame. CollisionObject* claimCollisionObject(const dynamics::ShapeFrame* shapeFrame); // TODO(JS): Maybe WeakShapeFramePtr + /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject + /// will be destroyed if no CollisionGroup holds it. + void reclaimCollisionObject(const CollisionObject* shapeFrame); + /// Create CollisionObject virtual std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) = 0; - /// + /// Notify that a CollisionObject will be destroyed so that the collision + /// detection engine do some relevant work. virtual void notifyCollisionObjectDestorying(CollisionObject* collObj) = 0; +protected: + + std::unique_ptr mCollisionObjectManager; + +}; + +class CollisionDetector::CollisionObjectManager +{ +public: + + /// Constructor + CollisionObjectManager(CollisionDetector* cd); + + /// Return CollisionObject associated with shapeFrame. New CollisionObject + /// will be created if it hasn't created yet for shapeFrame. + virtual CollisionObject* claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) = 0; + // TODO(JS): Maybe WeakShapeFramePtr + /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject /// will be destroyed if no CollisionGroup holds it. - void reclaimCollisionObject(const CollisionObject* shapeFrame); + virtual void reclaimCollisionObject(const CollisionObject* shapeFrame) = 0; + +protected: + + CollisionDetector* mCollisionDetector; + +}; + +class CollisionDetector::NaiveCollisionObjectManager : + public CollisionDetector::CollisionObjectManager +{ +public: + + /// Constructor + NaiveCollisionObjectManager(CollisionDetector* cd); + + /// Destructor + virtual ~NaiveCollisionObjectManager(); + + // Documentation inherited + CollisionObject* claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) override; + + // Documentation inherited + void reclaimCollisionObject(const CollisionObject* shapeFrame) override; + +protected: + + std::vector> mCollisionObjects; + +}; + +class CollisionDetector::RefCountingCollisionObjectManager : + public CollisionDetector::CollisionObjectManager +{ +public: + + /// Constructor + RefCountingCollisionObjectManager(CollisionDetector* cd); + + /// Destructor + virtual ~RefCountingCollisionObjectManager(); + + // Documentation inherited + CollisionObject* claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) override; + + // Documentation inherited + void reclaimCollisionObject(const CollisionObject* shapeFrame) override; protected: diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 3a5ee918c41f4..8380594ad9fda 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -213,7 +213,7 @@ bool BulletCollisionDetector::detect( //============================================================================== bool BulletCollisionDetector::detect( CollisionGroup* group1, CollisionGroup* group2, - const Option& /*option*/, Result& result) + const Option& option, Result& result) { result.contacts.clear(); @@ -232,22 +232,29 @@ bool BulletCollisionDetector::detect( return false; } - auto castedData1 = static_cast(group1); - auto castedData2 = static_cast(group2); + auto group = common::make_unique(shared_from_this()); + group->registerShapeFramesFrom(group1, group2); + group->update(); - auto bulletCollisionWorld1 = castedData1->getBulletCollisionWorld(); - auto bulletCollisionWorld2 = castedData2->getBulletCollisionWorld(); + auto bulletCollisionWorld = group->getBulletCollisionWorld(); + auto bulletPairCache = bulletCollisionWorld->getPairCache(); + auto filterCallback + = new BulletOverlapFilterCallback(option.collisionFilter.get()); - mBulletCollisionGroupForSinglePair->unregisterAllShapeFrames(); - mBulletCollisionGroupForSinglePair->registerShapeFramesFrom(group1, group2); + bulletPairCache->setOverlapFilterCallback(filterCallback); + bulletCollisionWorld->performDiscreteCollisionDetection(); -// BulletCollisionData collData(&option, &result); -// bulletCollisionWorld1->collide(bulletCollisionWorld2, &collData, checkPair); - // TODO(JS) + convertContacts(bulletCollisionWorld, result); return !result.contacts.empty(); } +//============================================================================== +BulletCollisionDetector::BulletCollisionDetector() +{ + mCollisionObjectManager.reset(new NaiveCollisionObjectManager(this)); +} + //============================================================================== std::unique_ptr BulletCollisionDetector::createCollisionObject( diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 8d112ee25056a..ef5c0c7dd734c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -93,7 +93,7 @@ class BulletCollisionDetector : public CollisionDetector }; /// Constructor - BulletCollisionDetector() = default; + BulletCollisionDetector(); // Documentation inherited std::unique_ptr createCollisionObject( @@ -133,9 +133,6 @@ class BulletCollisionDetector : public CollisionDetector std::map mBulletCollisionObjectMap; - std::shared_ptr mBulletCollisionGroupForSinglePair; - // TODO(JS): should be unique_ptr - }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 8948f157609fe..4c9c75674f1f5 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -46,10 +46,8 @@ namespace collision { BulletCollisionGroup::BulletCollisionGroup( const CollisionDetectorPtr& collisionDetector) : CollisionGroup(collisionDetector), - mBulletProadphaseAlg( - new btDbvtBroadphase()), - mBulletCollisionConfiguration( - new btDefaultCollisionConfiguration()), + mBulletProadphaseAlg(new btDbvtBroadphase()), + mBulletCollisionConfiguration(new btDefaultCollisionConfiguration()), mBulletDispatcher( new btCollisionDispatcher(mBulletCollisionConfiguration.get())), mBulletCollisionWorld( @@ -65,10 +63,8 @@ BulletCollisionGroup::BulletCollisionGroup( const CollisionDetectorPtr& collisionDetector, const dynamics::ShapeFrame* shapeFrame) : CollisionGroup(collisionDetector), - mBulletProadphaseAlg( - new btDbvtBroadphase()), - mBulletCollisionConfiguration( - new btDefaultCollisionConfiguration()), + mBulletProadphaseAlg(new btDbvtBroadphase()), + mBulletCollisionConfiguration(new btDefaultCollisionConfiguration()), mBulletDispatcher( new btCollisionDispatcher(mBulletCollisionConfiguration.get())), mBulletCollisionWorld( @@ -124,7 +120,7 @@ void BulletCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) mBulletCollisionWorld->addCollisionObject(casted->getBulletCollisionObject()); - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== @@ -139,7 +135,7 @@ void BulletCollisionGroup::notifyCollisionObjectsAdded( casted->getBulletCollisionObject()); } - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== @@ -151,7 +147,7 @@ void BulletCollisionGroup::notifyCollisionObjectRemoved( mBulletCollisionWorld->removeCollisionObject( casted->getBulletCollisionObject()); - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== @@ -160,7 +156,7 @@ void BulletCollisionGroup::notifyAllCollisionObjectsRemoved() for (const auto& collisionObject : getCollisionObjects()) notifyCollisionObjectRemoved(collisionObject); - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== diff --git a/dart/collision/bullet/BulletCollisionObject.cpp b/dart/collision/bullet/BulletCollisionObject.cpp index 25de64b4c6f98..cb64cc827a5c4 100644 --- a/dart/collision/bullet/BulletCollisionObject.cpp +++ b/dart/collision/bullet/BulletCollisionObject.cpp @@ -74,7 +74,7 @@ BulletCollisionObject::BulletCollisionObject( : CollisionObject(collisionDetector, shapeFrame), mBulletCollisionObjectUserData(new UserData(this, mCollisionDetector, nullptr)), - mBulletCollisionObject(new btCollisionObject()) + mBulletCollisionObject(common::make_unique()) { mBulletCollisionObject->setCollisionShape(bulletCollisionShape); mBulletCollisionObject->setUserPointer(mBulletCollisionObjectUserData.get()); diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index f3accf1d3df86..b7b037411d2f3 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -166,6 +166,12 @@ bool DARTCollisionDetector::detect( return !result.contacts.empty(); } +//============================================================================== +DARTCollisionDetector::DARTCollisionDetector() +{ + mCollisionObjectManager.reset(new RefCountingCollisionObjectManager(this)); +} + //============================================================================== std::unique_ptr DARTCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 7b64fe0a690f1..99211aefa11de 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -79,7 +79,7 @@ class DARTCollisionDetector : public CollisionDetector protected: /// Constructor - DARTCollisionDetector() = default; + DARTCollisionDetector(); // Documentation inherited std::unique_ptr createCollisionObject( diff --git a/dart/collision/detail/CollisionGroup.h b/dart/collision/detail/CollisionGroup.h index 4c8f05bdbc950..beda5ef3b2a39 100644 --- a/dart/collision/detail/CollisionGroup.h +++ b/dart/collision/detail/CollisionGroup.h @@ -48,11 +48,11 @@ template void CollisionGroup::registerShapeFramesFrom(const CollisionGroup* first, const Others*... others) { - if (!first) - return; - - for (const auto& shapeFrame : first->mShapeFrames) - registerShapeFrame(shapeFrame); + if (first && this != first) + { + for (const auto& shapeFrame : first->mShapeFrames) + registerShapeFrame(shapeFrame); + } registerShapeFramesFrom(others...); } @@ -62,13 +62,19 @@ template void CollisionGroup::unregisterShapeFramesFrom(const CollisionGroup* first, const Others*... others) { - if (!first) - return; + if (first) + { + if (this == first) + { + unregisterAllShapeFrames(); + return; + } - for (const auto& shapeFrame : first->mShapeFrames) + for (const auto& shapeFrame : first->mShapeFrames) unregisterShapeFrame(shapeFrame); + } - registerShapeFramesFrom(others...); + unregisterShapeFramesFrom(others...); } } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 2322094be794f..c8186814c035a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -864,12 +864,13 @@ FCLCollisionDetector::FCLCollisionDetector() : mPrimitiveShapeType(MESH), mContactPointComputationMethod(DART) { - // Do nothing + mCollisionObjectManager.reset(new RefCountingCollisionObjectManager(this)); } //============================================================================== std::unique_ptr -FCLCollisionDetector::createCollisionObject(const dynamics::ShapeFrame* shapeFrame) +FCLCollisionDetector::createCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { auto fclCollGeom = claimFCLCollisionGeometry(shapeFrame->getShape()); auto collObj = new FCLCollisionObject(this, shapeFrame, fclCollGeom); diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index d80fe3cca5f41..96defffde2544 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -94,7 +94,7 @@ void FCLCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) auto casted = static_cast(object); mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== @@ -108,7 +108,7 @@ void FCLCollisionGroup::notifyCollisionObjectsAdded( mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); } - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== @@ -118,7 +118,7 @@ void FCLCollisionGroup::notifyCollisionObjectRemoved(CollisionObject* object) mBroadPhaseAlg->unregisterObject(casted->getFCLCollisionObject()); - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== @@ -126,7 +126,7 @@ void FCLCollisionGroup::notifyAllCollisionObjectsRemoved() { mBroadPhaseAlg->clear(); - this->initializeEngineData(); + initializeEngineData(); } //============================================================================== diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 33d9188b16bfd..dcb323111b5bd 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -534,9 +534,7 @@ void testSimpleFrames() auto group3 = cd->createCollisionGroup(simpleFrame3.get()); auto groupAll = cd->createCollisionGroup(); - groupAll->registerShapeFramesFrom(group1.get()); - groupAll->registerShapeFramesFrom(group2.get()); - groupAll->registerShapeFramesFrom(group3.get()); + groupAll->registerShapeFramesFrom(group1.get(), group2.get(), group3.get()); EXPECT_EQ(group1->getNumShapeFrames(), 1u); EXPECT_EQ(group2->getNumShapeFrames(), 1u); @@ -549,16 +547,12 @@ void testSimpleFrames() collision::Option option; collision::Result result; - EXPECT_FALSE(group1->detect(option, result)); - EXPECT_FALSE(group2->detect(option, result)); - EXPECT_FALSE(group3->detect(option, result)); - simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); - EXPECT_FALSE(group1->detect(group2.get(), option, result)); - EXPECT_FALSE(group1->detect(group3.get(), option, result)); - EXPECT_FALSE(group2->detect(group3.get(), option, result)); + EXPECT_FALSE(group1->detect(option, result)); + EXPECT_FALSE(group2->detect(option, result)); + EXPECT_FALSE(group3->detect(option, result)); EXPECT_FALSE(groupAll->detect(option, result)); simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); @@ -577,7 +571,7 @@ TEST_F(COLLISION, SimpleFrames) // testSimpleFrames(); testSimpleFrames(); #ifdef HAVE_BULLET_COLLISION -// testSimpleFrames(); + testSimpleFrames(); #endif } @@ -613,8 +607,7 @@ void testBoxBox(const std::shared_ptr cd, double tol = 1e-12) auto group1 = cd->createCollisionGroup(simpleFrame1.get()); auto group2 = cd->createCollisionGroup(simpleFrame2.get()); auto groupAll = cd->createCollisionGroup(); - groupAll->registerShapeFramesFrom(group1.get()); - groupAll->registerShapeFramesFrom(group2.get()); + groupAll->registerShapeFramesFrom(group1.get(), group2.get()); EXPECT_EQ(group1->getNumShapeFrames(), 1u); EXPECT_EQ(group2->getNumShapeFrames(), 1u); @@ -639,8 +632,6 @@ void testBoxBox(const std::shared_ptr cd, double tol = 1e-12) const auto& contact = result.contacts[i]; const auto& point = contact.point; - std::cout << point.transpose() << std::endl; - EXPECT_TRUE(checkBoundingBox(min, max, point, tol)); } } @@ -708,7 +699,7 @@ TEST_F(COLLISION, BodyNodeNodes) // testBodyNodes(); testBodyNodes(); #ifdef HAVE_BULLET_COLLISION -// testBodyNodes(); + testBodyNodes(); #endif } From c0d83266b840ad68cba9f1fd533a34397be8e849 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 16:56:10 -0400 Subject: [PATCH 31/67] Add warning message for using FCL primitive shapes and FCL contact point computation because they are buggy yet --- dart/collision/fcl/FCLCollisionDetector.cpp | 21 +++ unittests/testCollision.cpp | 139 +++++++++++++++----- 2 files changed, 124 insertions(+), 36 deletions(-) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index c8186814c035a..92d1ca1afe290 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -835,6 +835,16 @@ bool FCLCollisionDetector::detect( void FCLCollisionDetector::setPrimitiveShapeType( FCLCollisionDetector::PrimitiveShape_t type) { + if (type == PRIMITIVE) + { + dtwarn << "[FCLCollisionDetector::setPrimitiveShapeType] You chose to use " + << "FCL's primitive shape collision feature while it's not complete " + << "(at least until 0.4.0) especially in use of dynamics " + << "simulation. It's recommended to use mesh even for primitive " + << "shapes by settting " + << "FCLCollisionDetector::setPrimitiveShapeType(MESH).\n"; + } + mPrimitiveShapeType = type; } @@ -849,6 +859,17 @@ FCLCollisionDetector::getPrimitiveShapeType() const void FCLCollisionDetector::setContactPointComputationMethod( FCLCollisionDetector::ContactPointComputationMethod_t method) { + if (method == FCL) + { + dtwarn << "[FCLCollisionDetector::setContactPointComputationMethod] You " + << "chose to use FCL's built in contact point computation while" + << "it's buggy (see https://github.com/flexible-collision-library/" + << "fcl/issues/106) at least until 0.4.0. It's recommended to use " + << "DART's implementation for the contact point computation by " + << "settting " + << "FCLCollisionDetector::setContactPointComputationMethod(DART).\n"; + } + mContactPointComputationMethod = method; } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index dcb323111b5bd..3185a650e3cf6 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -512,11 +512,8 @@ TEST_F(COLLISION, FCL_BOX_BOX) //} //============================================================================== -template -void testSimpleFrames() +void testSimpleFrames(const std::shared_ptr& cd) { - auto cd = CollisionDetectorType::create(); - auto simpleFrame1 = std::make_shared(Frame::World()); auto simpleFrame2 = std::make_shared(Frame::World()); auto simpleFrame3 = std::make_shared(Frame::World()); @@ -567,12 +564,33 @@ void testSimpleFrames() //============================================================================== TEST_F(COLLISION, SimpleFrames) { - testSimpleFrames(); -// testSimpleFrames(); - testSimpleFrames(); + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testSimpleFrames(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testSimpleFrames(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testSimpleFrames(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testSimpleFrames(fcl_mesh_fcl); + #ifdef HAVE_BULLET_COLLISION - testSimpleFrames(); + auto bullet = BulletCollisionDetector::create(); + testSimpleFrames(bullet); #endif + + auto dart = DARTCollisionDetector::create(); + testSimpleFrames(dart); } //============================================================================== @@ -589,7 +607,8 @@ bool checkBoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max, } //============================================================================== -void testBoxBox(const std::shared_ptr cd, double tol = 1e-12) +void testBoxBox(const std::shared_ptr& cd, + double tol = 1e-12) { auto simpleFrame1 = std::make_shared(Frame::World()); auto simpleFrame2 = std::make_shared(Frame::World()); @@ -625,34 +644,46 @@ void testBoxBox(const std::shared_ptr cd, double tol = 1e-12) const auto numContacts = result.contacts.size(); - EXPECT_TRUE(numContacts <= 4u); + const auto checkNumContacts = (numContacts <= 4u); + EXPECT_TRUE(checkNumContacts); + if (!checkNumContacts) + std::cout << "# of contants: " << numContacts << "\n"; for (auto i = 0u; i < numContacts; ++i) { const auto& contact = result.contacts[i]; const auto& point = contact.point; - EXPECT_TRUE(checkBoundingBox(min, max, point, tol)); + const auto result = checkBoundingBox(min, max, point, tol); + EXPECT_TRUE(result); + + if (!result) + std::cout << "point: " << point.transpose() << "\n"; } } //============================================================================== TEST_F(COLLISION, BoxBox) { - auto fcl_prim_fcl = FCLCollisionDetector::create(); - fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); - fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); - testBoxBox(fcl_prim_fcl); - auto fcl_mesh_dart = FCLCollisionDetector::create(); fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); testBoxBox(fcl_mesh_dart); - auto fcl_mesh_fcl = FCLCollisionDetector::create(); - fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); - fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); -// testBoxBox(fcl_mesh_fcl); + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testBoxBox(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testBoxBox(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testBoxBox(fcl_mesh_fcl); #ifdef HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); @@ -664,11 +695,8 @@ TEST_F(COLLISION, BoxBox) } //============================================================================== -template -void testBodyNodes() +void testBodyNodes(const std::shared_ptr& cd) { - auto cd = CollisionDetectorType::create(); - Eigen::Vector3d size(1.0, 1.0, 1.0); Eigen::Vector3d pos1(0.0, 0.0, 0.0); Eigen::Vector3d pos2(0.5, 0.0, 0.0); @@ -695,20 +723,38 @@ void testBodyNodes() //============================================================================== TEST_F(COLLISION, BodyNodeNodes) { - testBodyNodes(); -// testBodyNodes(); - testBodyNodes(); + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testBodyNodes(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testBodyNodes(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testBodyNodes(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testBodyNodes(fcl_mesh_fcl); + #ifdef HAVE_BULLET_COLLISION - testBodyNodes(); + auto bullet = BulletCollisionDetector::create(); + testBodyNodes(bullet); #endif + + auto dart = DARTCollisionDetector::create(); + testBodyNodes(dart); } //============================================================================== -template -void testSkeletons() +void testSkeletons(const std::shared_ptr& cd) { - auto cd = CollisionDetectorType::create(); - Eigen::Vector3d size(1.0, 1.0, 1.0); Eigen::Vector3d pos1(0.0, 0.0, 0.0); Eigen::Vector3d pos2(0.5, 0.0, 0.0); @@ -727,12 +773,33 @@ void testSkeletons() //============================================================================== TEST_F(COLLISION, Skeletons) { - testSkeletons(); -// testSkeletons(); - testSkeletons(); + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testSkeletons(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testSkeletons(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testSkeletons(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testSkeletons(fcl_mesh_fcl); + #ifdef HAVE_BULLET_COLLISION - testSkeletons(); + auto bullet = BulletCollisionDetector::create(); + testSkeletons(bullet); #endif + + auto dart = DARTCollisionDetector::create(); + testSkeletons(dart); } //============================================================================== From 12743da18fbf1e69ffdc1d048abaa574a21b0455 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 18:49:46 -0400 Subject: [PATCH 32/67] Use fcl 0.3.2 for homebrew build test --- ci/before_install_osx.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ci/before_install_osx.sh b/ci/before_install_osx.sh index e80330ad26dc9..3a3357142d204 100755 --- a/ci/before_install_osx.sh +++ b/ci/before_install_osx.sh @@ -7,7 +7,7 @@ PACKAGES=' git cmake assimp -fcl +fcl03 bullet flann boost From 19fdc4b317697b596eb503da3a7a18db2b4ac80e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 18:55:56 -0400 Subject: [PATCH 33/67] Disable appveyor build test unless Visual Studio 2015 is capable to build with `constexpr` for function pointer --- appveyor.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index e33e34b9a07af..9cc61dae6f060 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -57,10 +57,10 @@ before_build: - cmd: if "%platform%"=="x64" set CMAKE_GENERATOR_NAME=Visual Studio 14 2015 Win64 - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%configuration% -DBUILD_CORE_ONLY="%BUILD_CORE_ONLY%" -DDART_BUILD_EXAMPLES="%BUILD_EXAMPLES%" -DDART_BUILD_TUTORIALS="%BUILD_TUTORIALS%" -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" -DBoost_USE_STATIC_LIBS="ON" -Durdfdom_DIR="%urdfdom_DIR%" -Durdfdom_headers_DIR="%urdfdom_headers_DIR%" -DDART_MSVC_DEFAULT_OPTIONS="%MSVC_DEFAULT_OPTIONS%" .. -build: - project: C:\projects\dart\build\dart.sln # path to Visual Studio solution or project - parallel: true # enable MSBuild parallel builds - verbosity: quiet # MSBuild verbosity level (quiet|minimal|normal|detailed) +#build: +# project: C:\projects\dart\build\dart.sln # path to Visual Studio solution or project +# parallel: true # enable MSBuild parallel builds +# verbosity: quiet # MSBuild verbosity level (quiet|minimal|normal|detailed) -test_script: - - cmd: ctest --build-config %configuration% --parallel 4 --output-on-failure +#test_script: +# - cmd: ctest --build-config %configuration% --parallel 4 --output-on-failure From a18f4ddfbd30b290b17e3a4d69ddd8acfc30d76f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 19:05:11 -0400 Subject: [PATCH 34/67] Disable automatic build/tests specifically in appveyor.yml --- appveyor.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/appveyor.yml b/appveyor.yml index 9cc61dae6f060..fec6ee9dabdfd 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -57,10 +57,12 @@ before_build: - cmd: if "%platform%"=="x64" set CMAKE_GENERATOR_NAME=Visual Studio 14 2015 Win64 - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%configuration% -DBUILD_CORE_ONLY="%BUILD_CORE_ONLY%" -DDART_BUILD_EXAMPLES="%BUILD_EXAMPLES%" -DDART_BUILD_TUTORIALS="%BUILD_TUTORIALS%" -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" -DBoost_USE_STATIC_LIBS="ON" -Durdfdom_DIR="%urdfdom_DIR%" -Durdfdom_headers_DIR="%urdfdom_headers_DIR%" -DDART_MSVC_DEFAULT_OPTIONS="%MSVC_DEFAULT_OPTIONS%" .. +build: off #build: # project: C:\projects\dart\build\dart.sln # path to Visual Studio solution or project # parallel: true # enable MSBuild parallel builds # verbosity: quiet # MSBuild verbosity level (quiet|minimal|normal|detailed) +test: off #test_script: # - cmd: ctest --build-config %configuration% --parallel 4 --output-on-failure From efc9699cd94722e07ca723f982d31908688432bf Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 19:37:20 -0400 Subject: [PATCH 35/67] Use mesh version of fcl collision detector by default in ConstraintSolver until incorrect contact point computation of FCL is resolved. --- dart/constraint/ConstraintSolver.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 80bca5f32e1fd..502e40ca07141 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -76,11 +76,10 @@ ConstraintSolver::ConstraintSolver(double timeStep) auto cd = std::static_pointer_cast( mCollisionDetector); -#if DART_USE_FCLMESHCOLLISIONDETECTOR cd->setPrimitiveShapeType(collision::FCLCollisionDetector::MESH); -#else - cd->setPrimitiveShapeType(collision::FCLCollisionDetector::PRIMITIVE); -#endif + // TODO(JS): Consider using FCL's primitive shapes once FCL addresses + // incorrect contact point computation. + // (see: https://github.com/flexible-collision-library/fcl/issues/106) } //============================================================================== From 0a265e6fe81ae5184c866588884e5b9295c84fdf Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 26 Mar 2016 21:06:33 -0400 Subject: [PATCH 36/67] Polish FCL/Bullet/DARTCollisionDetector classes --- dart/collision/CollisionDetector.cpp | 2 - .../bullet/BulletCollisionDetector.cpp | 294 +++++++++++++----- .../bullet/BulletCollisionDetector.h | 9 +- .../bullet/BulletCollisionObject.cpp | 28 +- dart/collision/bullet/BulletCollisionObject.h | 19 +- dart/collision/dart/DARTCollide.cpp | 114 ++----- dart/collision/dart/DARTCollisionDetector.cpp | 35 ++- dart/collision/dart/DARTCollisionDetector.h | 2 +- dart/collision/fcl/CollisionShapes.h | 1 - dart/collision/fcl/FCLCollisionDetector.cpp | 40 +-- dart/collision/fcl/FCLCollisionDetector.h | 8 - dart/collision/fcl/FCLCollisionObject.cpp | 2 - dart/collision/fcl/FCLCollisionObject.h | 4 - 13 files changed, 307 insertions(+), 251 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index de6baccc6ef32..f7c076390b182 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -38,8 +38,6 @@ #include "dart/collision/CollisionDetector.h" #include -#include -#include #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 8380594ad9fda..cda676665c257 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -36,7 +36,7 @@ #include "dart/collision/bullet/BulletCollisionDetector.h" -#include +#include #include "dart/common/Console.h" #include "dart/collision/CollisionObject.h" @@ -45,11 +45,11 @@ #include "dart/collision/bullet/BulletCollisionObject.h" #include "dart/collision/bullet/BulletCollisionGroup.h" #include "dart/dynamics/ShapeFrame.h" +#include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/EllipsoidShape.h" #include "dart/dynamics/CylinderShape.h" #include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" #include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" @@ -90,15 +90,8 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback auto userData1 = static_cast( bulletCollObj1->getUserPointer()); - auto collisionDetector = userData0->collisionDetector; - assert(collisionDetector == userData1->collisionDetector); - - auto castedCD = static_cast(collisionDetector); - - auto collObj0 = castedCD->findCollisionObject(bulletCollObj0); - auto collObj1 = castedCD->findCollisionObject(bulletCollObj1); - - collide = mFilter->needCollision(collObj0, collObj1); + collide = mFilter->needCollision(userData0->collisionObject, + userData1->collisionObject); } return collide; @@ -107,21 +100,6 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback CollisionFilter* mFilter; }; -struct BulletContactResultCallback : btCollisionWorld::ContactResultCallback -{ - BulletContactResultCallback(Result& result); - - btScalar addSingleResult(btManifoldPoint& cp, - const btCollisionObjectWrapper* colObj0Wrap, - int partId0, - int index0, - const btCollisionObjectWrapper* colObj1Wrap, - int partId1, - int index1) override; - - Result& mResult; -}; - Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData1, const BulletCollisionObject::UserData* userData2); @@ -270,17 +248,6 @@ BulletCollisionDetector::createCollisionObject( return std::unique_ptr(collObj); } -//============================================================================== -BulletCollisionObject* BulletCollisionDetector::findCollisionObject( - btCollisionObject* bulletCollObj) const -{ - auto search = mBulletCollisionObjectMap.find(bulletCollObj); - if (mBulletCollisionObjectMap.end() != search) - return search->second; - else - return nullptr; -} - //============================================================================== void BulletCollisionDetector::notifyCollisionObjectDestorying( CollisionObject* collObj) @@ -337,6 +304,220 @@ void BulletCollisionDetector::reclaimBulletCollisionGeometry( } } +//============================================================================== +BulletCollisionDetector::BulletCollsionPack +BulletCollisionDetector::createEllipsoidMesh( + float sizeX, float sizeY, float sizeZ) +{ + float v[59][3] = + { + {0, 0, 0}, + {0.135299, -0.461940, -0.135299}, + {0.000000, -0.461940, -0.191342}, + {-0.135299, -0.461940, -0.135299}, + {-0.191342, -0.461940, 0.000000}, + {-0.135299, -0.461940, 0.135299}, + {0.000000, -0.461940, 0.191342}, + {0.135299, -0.461940, 0.135299}, + {0.191342, -0.461940, 0.000000}, + {0.250000, -0.353553, -0.250000}, + {0.000000, -0.353553, -0.353553}, + {-0.250000, -0.353553, -0.250000}, + {-0.353553, -0.353553, 0.000000}, + {-0.250000, -0.353553, 0.250000}, + {0.000000, -0.353553, 0.353553}, + {0.250000, -0.353553, 0.250000}, + {0.353553, -0.353553, 0.000000}, + {0.326641, -0.191342, -0.326641}, + {0.000000, -0.191342, -0.461940}, + {-0.326641, -0.191342, -0.326641}, + {-0.461940, -0.191342, 0.000000}, + {-0.326641, -0.191342, 0.326641}, + {0.000000, -0.191342, 0.461940}, + {0.326641, -0.191342, 0.326641}, + {0.461940, -0.191342, 0.000000}, + {0.353553, 0.000000, -0.353553}, + {0.000000, 0.000000, -0.500000}, + {-0.353553, 0.000000, -0.353553}, + {-0.500000, 0.000000, 0.000000}, + {-0.353553, 0.000000, 0.353553}, + {0.000000, 0.000000, 0.500000}, + {0.353553, 0.000000, 0.353553}, + {0.500000, 0.000000, 0.000000}, + {0.326641, 0.191342, -0.326641}, + {0.000000, 0.191342, -0.461940}, + {-0.326641, 0.191342, -0.326641}, + {-0.461940, 0.191342, 0.000000}, + {-0.326641, 0.191342, 0.326641}, + {0.000000, 0.191342, 0.461940}, + {0.326641, 0.191342, 0.326641}, + {0.461940, 0.191342, 0.000000}, + {0.250000, 0.353553, -0.250000}, + {0.000000, 0.353553, -0.353553}, + {-0.250000, 0.353553, -0.250000}, + {-0.353553, 0.353553, 0.000000}, + {-0.250000, 0.353553, 0.250000}, + {0.000000, 0.353553, 0.353553}, + {0.250000, 0.353553, 0.250000}, + {0.353553, 0.353553, 0.000000}, + {0.135299, 0.461940, -0.135299}, + {0.000000, 0.461940, -0.191342}, + {-0.135299, 0.461940, -0.135299}, + {-0.191342, 0.461940, 0.000000}, + {-0.135299, 0.461940, 0.135299}, + {0.000000, 0.461940, 0.191342}, + {0.135299, 0.461940, 0.135299}, + {0.191342, 0.461940, 0.000000}, + {0.000000, -0.500000, 0.000000}, + {0.000000, 0.500000, 0.000000} + }; + + int f[112][3] = + { + {1, 2, 9}, + {9, 2, 10}, + {2, 3, 10}, + {10, 3, 11}, + {3, 4, 11}, + {11, 4, 12}, + {4, 5, 12}, + {12, 5, 13}, + {5, 6, 13}, + {13, 6, 14}, + {6, 7, 14}, + {14, 7, 15}, + {7, 8, 15}, + {15, 8, 16}, + {8, 1, 16}, + {16, 1, 9}, + {9, 10, 17}, + {17, 10, 18}, + {10, 11, 18}, + {18, 11, 19}, + {11, 12, 19}, + {19, 12, 20}, + {12, 13, 20}, + {20, 13, 21}, + {13, 14, 21}, + {21, 14, 22}, + {14, 15, 22}, + {22, 15, 23}, + {15, 16, 23}, + {23, 16, 24}, + {16, 9, 24}, + {24, 9, 17}, + {17, 18, 25}, + {25, 18, 26}, + {18, 19, 26}, + {26, 19, 27}, + {19, 20, 27}, + {27, 20, 28}, + {20, 21, 28}, + {28, 21, 29}, + {21, 22, 29}, + {29, 22, 30}, + {22, 23, 30}, + {30, 23, 31}, + {23, 24, 31}, + {31, 24, 32}, + {24, 17, 32}, + {32, 17, 25}, + {25, 26, 33}, + {33, 26, 34}, + {26, 27, 34}, + {34, 27, 35}, + {27, 28, 35}, + {35, 28, 36}, + {28, 29, 36}, + {36, 29, 37}, + {29, 30, 37}, + {37, 30, 38}, + {30, 31, 38}, + {38, 31, 39}, + {31, 32, 39}, + {39, 32, 40}, + {32, 25, 40}, + {40, 25, 33}, + {33, 34, 41}, + {41, 34, 42}, + {34, 35, 42}, + {42, 35, 43}, + {35, 36, 43}, + {43, 36, 44}, + {36, 37, 44}, + {44, 37, 45}, + {37, 38, 45}, + {45, 38, 46}, + {38, 39, 46}, + {46, 39, 47}, + {39, 40, 47}, + {47, 40, 48}, + {40, 33, 48}, + {48, 33, 41}, + {41, 42, 49}, + {49, 42, 50}, + {42, 43, 50}, + {50, 43, 51}, + {43, 44, 51}, + {51, 44, 52}, + {44, 45, 52}, + {52, 45, 53}, + {45, 46, 53}, + {53, 46, 54}, + {46, 47, 54}, + {54, 47, 55}, + {47, 48, 55}, + {55, 48, 56}, + {48, 41, 56}, + {56, 41, 49}, + {2, 1, 57}, + {3, 2, 57}, + {4, 3, 57}, + {5, 4, 57}, + {6, 5, 57}, + {7, 6, 57}, + {8, 7, 57}, + {1, 8, 57}, + {49, 50, 58}, + {50, 51, 58}, + {51, 52, 58}, + {52, 53, 58}, + {53, 54, 58}, + {54, 55, 58}, + {55, 56, 58}, + {56, 49, 58} + }; + + BulletCollsionPack pack; + pack.triMesh.reset(new btTriangleMesh()); + + for (auto i = 0u; i < 112; ++i) + { + btVector3 vertices[3]; + + const auto& index0 = f[i][0]; + const auto& index1 = f[i][1]; + const auto& index2 = f[i][2]; + + const auto& p0 = v[index0]; + const auto& p1 = v[index1]; + const auto& p2 = v[index2]; + + vertices[0] = btVector3(p0[0] * sizeX, p0[1] * sizeY, p0[2] * sizeZ); + vertices[1] = btVector3(p1[0] * sizeX, p1[1] * sizeY, p1[2] * sizeZ); + vertices[2] = btVector3(p2[0] * sizeX, p2[1] * sizeY, p2[2] * sizeZ); + + pack.triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + } + + auto gimpactMeshShape = new btGImpactMeshShape(pack.triMesh.get()); + gimpactMeshShape->updateBound(); + + pack.collisionShape.reset(gimpactMeshShape); + + return pack; +} + //============================================================================== BulletCollisionDetector::BulletCollsionPack BulletCollisionDetector::createMesh( @@ -512,45 +693,6 @@ BulletCollisionDetector::createBulletCollisionShape( namespace { -//============================================================================== -BulletContactResultCallback::BulletContactResultCallback(Result& result) - : ContactResultCallback(), - mResult(result) -{ - // Do nothing -} - -//============================================================================== -//bool BulletContactResultCallback::needsCollision( -// btBroadphaseProxy* proxy0) const -//{ -// return true; -//} - -//============================================================================== -btScalar BulletContactResultCallback::addSingleResult( - btManifoldPoint& cp, - const btCollisionObjectWrapper* colObj0Wrap, - int /*partId0*/, - int /*index0*/, - const btCollisionObjectWrapper* colObj1Wrap, - int /*partId1*/, - int /*index1*/) -{ - auto userPointer0 = colObj0Wrap->getCollisionObject()->getUserPointer(); - auto userPointer1 = colObj1Wrap->getCollisionObject()->getUserPointer(); - - auto userDataA - = static_cast(userPointer1); - auto userDataB - = static_cast(userPointer0); - - mResult.contacts.push_back(convertContact(cp, userDataA, userDataB)); - - return 1.0f; -} - - //============================================================================== Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData1, diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index ef5c0c7dd734c..9f55a331b3f6e 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -99,13 +99,6 @@ class BulletCollisionDetector : public CollisionDetector std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; -public: - - BulletCollisionObject* findCollisionObject( - btCollisionObject* bulletCollObj) const; - -protected: - // Documentation inherited void notifyCollisionObjectDestorying(CollisionObject* collObj) override; @@ -116,6 +109,8 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited void reclaimBulletCollisionGeometry(const dynamics::ConstShapePtr& shape); + BulletCollsionPack createEllipsoidMesh(float sizeX, float sizeY, float sizeZ); + BulletCollsionPack createMesh( const Eigen::Vector3d& scale, const aiScene* mesh); diff --git a/dart/collision/bullet/BulletCollisionObject.cpp b/dart/collision/bullet/BulletCollisionObject.cpp index cb64cc827a5c4..64af9caa951e9 100644 --- a/dart/collision/bullet/BulletCollisionObject.cpp +++ b/dart/collision/bullet/BulletCollisionObject.cpp @@ -36,32 +36,27 @@ #include "dart/collision/bullet/BulletCollisionObject.h" -#include "dart/common/Console.h" #include "dart/collision/bullet/BulletTypes.h" -#include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionObject.h" -#include "dart/collision/bullet/BulletTypes.h" -#include "dart/dynamics/Shape.h" #include "dart/dynamics/ShapeFrame.h" -#include "dart/dynamics/SoftMeshShape.h" namespace dart { namespace collision { //============================================================================== -BulletCollisionObject::UserData::UserData( - CollisionObject* collisionObject, - CollisionDetector* collisionDetector, - CollisionGroup* collisionGroup) - : collisionObject(collisionObject), - collisionDetector(collisionDetector), - group(collisionGroup) +BulletCollisionObject::UserData::UserData(CollisionObject* collisionObject) + : collisionObject(collisionObject) { // Do nothing } //============================================================================== -btCollisionObject* BulletCollisionObject::getBulletCollisionObject() const +btCollisionObject* BulletCollisionObject::getBulletCollisionObject() +{ + return mBulletCollisionObject.get(); +} + +//============================================================================== +const btCollisionObject* BulletCollisionObject::getBulletCollisionObject() const { return mBulletCollisionObject.get(); } @@ -72,9 +67,8 @@ BulletCollisionObject::BulletCollisionObject( const dynamics::ShapeFrame* shapeFrame, btCollisionShape* bulletCollisionShape) : CollisionObject(collisionDetector, shapeFrame), - mBulletCollisionObjectUserData(new UserData(this, mCollisionDetector, - nullptr)), - mBulletCollisionObject(common::make_unique()) + mBulletCollisionObjectUserData(new UserData(this)), + mBulletCollisionObject(new btCollisionObject()) { mBulletCollisionObject->setCollisionShape(bulletCollisionShape); mBulletCollisionObject->setUserPointer(mBulletCollisionObjectUserData.get()); diff --git a/dart/collision/bullet/BulletCollisionObject.h b/dart/collision/bullet/BulletCollisionObject.h index 7c5795a3dd723..cd0374e471eb9 100644 --- a/dart/collision/bullet/BulletCollisionObject.h +++ b/dart/collision/bullet/BulletCollisionObject.h @@ -37,12 +37,7 @@ #ifndef DART_COLLISION_BULLET_BULLETCOLLISIONOBJECT_H_ #define DART_COLLISION_BULLET_BULLETCOLLISIONOBJECT_H_ -#include -#include -#include -#include #include -#include #include "dart/collision/CollisionObject.h" namespace dart { @@ -58,18 +53,16 @@ class BulletCollisionObject : public CollisionObject { CollisionObject* collisionObject; - CollisionDetector* collisionDetector; - CollisionGroup* group; - - UserData(CollisionObject* collisionObject, - CollisionDetector* collisionDetector, - CollisionGroup* collisionGroup); + UserData(CollisionObject* collisionObject); }; friend class BulletCollisionDetector; - /// Return FCL collision object - btCollisionObject* getBulletCollisionObject() const; + /// Return Bullet collision object + btCollisionObject* getBulletCollisionObject(); + + /// Return Bullet collision object + const btCollisionObject* getBulletCollisionObject() const; protected: diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 350f4cc42967f..99e06f8ae5a6a 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -1216,86 +1216,64 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const return 0; } -int collide(dynamics::ConstShapePtr _shape0, const Eigen::Isometry3d& _T0, - dynamics::ConstShapePtr _shape1, const Eigen::Isometry3d& _T1, - std::vector* _result) +//============================================================================== +int collide(dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, + dynamics::ConstShapePtr shape1, const Eigen::Isometry3d& T1, + std::vector* result) { - dynamics::Shape::ShapeType LeftType = _shape0->getShapeType(); - dynamics::Shape::ShapeType RightType = _shape1->getShapeType(); + dynamics::Shape::ShapeType LeftType = shape0->getShapeType(); + dynamics::Shape::ShapeType RightType = shape1->getShapeType(); switch(LeftType) { case dynamics::Shape::BOX: { - const dynamics::BoxShape* box0 = static_cast(_shape0.get()); + const dynamics::BoxShape* box0 = static_cast(shape0.get()); switch(RightType) { case dynamics::Shape::BOX: { - const dynamics::BoxShape* box1 = static_cast(_shape1.get()); - return collideBoxBox(box0->getSize(), _T0, - box1->getSize(), _T1, _result); + const dynamics::BoxShape* box1 = static_cast(shape1.get()); + return collideBoxBox(box0->getSize(), T0, + box1->getSize(), T1, result); } case dynamics::Shape::ELLIPSOID: { - const dynamics::EllipsoidShape* ellipsoid1 = static_cast(_shape1.get()); - return collideBoxSphere(box0->getSize(), _T0, - ellipsoid1->getSize()[0] * 0.5, _T1, - _result); - } - case dynamics::Shape::CYLINDER: - { - //---------------------------------------------------------- - // NOT SUPPORT CYLINDER - //---------------------------------------------------------- - const dynamics::CylinderShape* cylinder1 = static_cast(_shape1.get()); - - Eigen::Vector3d dimTemp(cylinder1->getRadius() * sqrt(2.0), - cylinder1->getRadius() * sqrt(2.0), - cylinder1->getHeight()); - return collideBoxBox(box0->getSize(), _T0, dimTemp, _T1, _result); + const dynamics::EllipsoidShape* ellipsoid1 = static_cast(shape1.get()); + return collideBoxSphere(box0->getSize(), T0, + ellipsoid1->getSize()[0] * 0.5, T1, + result); } default: + { return false; break; + } } break; } case dynamics::Shape::ELLIPSOID: { - const dynamics::EllipsoidShape* ellipsoid0 = static_cast(_shape0.get()); + const dynamics::EllipsoidShape* ellipsoid0 = static_cast(shape0.get()); switch(RightType) { case dynamics::Shape::BOX: { - const dynamics::BoxShape* box1 = static_cast(_shape1.get()); - return collideSphereBox(ellipsoid0->getSize()[0] * 0.5, _T0, - box1->getSize(), _T1, - _result); + const dynamics::BoxShape* box1 = static_cast(shape1.get()); + return collideSphereBox(ellipsoid0->getSize()[0] * 0.5, T0, + box1->getSize(), T1, + result); } case dynamics::Shape::ELLIPSOID: { - const dynamics::EllipsoidShape* ellipsoid1 = static_cast(_shape1.get()); - return collideSphereSphere(ellipsoid0->getSize()[0] * 0.5, _T0, - ellipsoid1->getSize()[0] * 0.5, _T1, - _result); - } - case dynamics::Shape::CYLINDER: - { - //---------------------------------------------------------- - // NOT SUPPORT CYLINDER - //---------------------------------------------------------- - const dynamics::CylinderShape* cylinder1 = static_cast(_shape1.get()); - - Eigen::Vector3d dimTemp1(cylinder1->getRadius() * sqrt(2.0), - cylinder1->getRadius() * sqrt(2.0), - cylinder1->getHeight()); - return collideSphereBox( - ellipsoid0->getSize()[0] * 0.5, _T0, dimTemp1, _T1, _result); + const dynamics::EllipsoidShape* ellipsoid1 = static_cast(shape1.get()); + return collideSphereSphere(ellipsoid0->getSize()[0] * 0.5, T0, + ellipsoid1->getSize()[0] * 0.5, T1, + result); } default: return false; @@ -1305,48 +1283,6 @@ int collide(dynamics::ConstShapePtr _shape0, const Eigen::Isometry3d& _T0, break; } - case dynamics::Shape::CYLINDER: - { - //---------------------------------------------------------- - // NOT SUPPORT CYLINDER - //---------------------------------------------------------- - const dynamics::CylinderShape* cylinder0 = static_cast(_shape0.get()); - - Eigen::Vector3d dimTemp0(cylinder0->getRadius() * sqrt(2.0), - cylinder0->getRadius() * sqrt(2.0), - cylinder0->getHeight()); - switch(RightType) - { - case dynamics::Shape::BOX: - { - const dynamics::BoxShape* box1 = static_cast(_shape1.get()); - return collideBoxBox(dimTemp0, _T0, box1->getSize(), _T1, _result); - } - case dynamics::Shape::ELLIPSOID: - { - const dynamics::EllipsoidShape* ellipsoid1 = static_cast(_shape1.get()); - return collideBoxSphere(dimTemp0, _T0, ellipsoid1->getSize()[0] * 0.5, _T1, _result); - } - case dynamics::Shape::CYLINDER: - { - //---------------------------------------------------------- - // NOT SUPPORT CYLINDER - //---------------------------------------------------------- - const dynamics::CylinderShape* cylinder1 = static_cast(_shape1.get()); - - Eigen::Vector3d dimTemp1(cylinder1->getRadius() * sqrt(2.0), - cylinder1->getRadius() * sqrt(2.0), - cylinder1->getHeight()); - return collideBoxBox(dimTemp0, _T0, dimTemp1, _T1, _result); - } - default: - { - return false; - } - } - - break; - } default: return false; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index b7b037411d2f3..7d209f533109f 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -36,11 +36,12 @@ #include "dart/collision/dart/DARTCollisionDetector.h" -#include #include "dart/collision/CollisionObject.h" +#include "dart/collision/CollisionFilter.h" #include "dart/collision/dart/DARTCollide.h" #include "dart/collision/dart/DARTCollisionObject.h" #include "dart/collision/dart/DARTCollisionGroup.h" +#include "dart/dynamics/ShapeFrame.h" namespace dart { namespace collision { @@ -102,7 +103,7 @@ std::shared_ptr DARTCollisionDetector::createCollisionGroup( //============================================================================== bool DARTCollisionDetector::detect( CollisionGroup* group, - const Option& /*option*/, Result& result) + const Option& option, Result& result) { result.contacts.clear(); @@ -115,6 +116,8 @@ bool DARTCollisionDetector::detect( if (objects.empty()) return false; + const auto& filter = option.collisionFilter; + for (auto i = 0u; i < objects.size() - 1; ++i) { auto collObj1 = objects[i]; @@ -123,6 +126,9 @@ bool DARTCollisionDetector::detect( { auto collObj2 = objects[j]; + if (filter && !filter->needCollision(collObj1, collObj2)) + continue; + checkPair(collObj1, collObj2, result); } } @@ -134,7 +140,7 @@ bool DARTCollisionDetector::detect( bool DARTCollisionDetector::detect( CollisionGroup* group1, CollisionGroup* group2, - const Option& /*option*/, Result& result) + const Option& option, Result& result) { result.contacts.clear(); @@ -151,6 +157,8 @@ bool DARTCollisionDetector::detect( if (objects1.empty() || objects2.empty()) return false; + const auto& filter = option.collisionFilter; + for (auto i = 0u; i < objects1.size(); ++i) { auto collObj1 = objects1[i]; @@ -159,6 +167,9 @@ bool DARTCollisionDetector::detect( { auto collObj2 = objects2[j]; + if (filter && !filter->needCollision(collObj1, collObj2)) + continue; + checkPair(collObj1, collObj2, result); } } @@ -172,12 +183,28 @@ DARTCollisionDetector::DARTCollisionDetector() mCollisionObjectManager.reset(new RefCountingCollisionObjectManager(this)); } +//============================================================================== +void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) +{ + if (!shapeFrame) + return; + + dterr << "[DARTCollisionDetector] Attempting to create shape type '" + << shapeFrame->getShape()->getShapeType() << "' that is not supported " + << "by DARTCollisionDetector. Currently, only BoxShape and " + << "EllipsoidShape (only when all the radii are equal) are " + << "supported. This shape will always get penetrated by other " + << "objects.\n"; +} + //============================================================================== std::unique_ptr DARTCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { auto collObj = new DARTCollisionObject(this, shapeFrame); + warnUnsupportedShapeType(shapeFrame); + mDARTCollisionObjects.push_back(collObj); return std::unique_ptr(collObj); @@ -203,8 +230,6 @@ namespace { //============================================================================== bool checkPair(CollisionObject* o1, CollisionObject* o2, Result& result) { - // TODO(JS): filtering - Result pairResult; // Perform narrow-phase detection diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 99211aefa11de..75eb8999fdad2 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -51,7 +51,7 @@ class DARTCollisionDetector : public CollisionDetector static std::shared_ptr create(); - /// Return engine type "FCLMesh" + /// Return engine type "DART" static const std::string& getTypeStatic(); // Documentation inherited diff --git a/dart/collision/fcl/CollisionShapes.h b/dart/collision/fcl/CollisionShapes.h index e9b6f8e731707..0635439352e7c 100644 --- a/dart/collision/fcl/CollisionShapes.h +++ b/dart/collision/fcl/CollisionShapes.h @@ -38,7 +38,6 @@ #define DART_COLLISION_FCL_MESH_COLLISIONSHAPES_H_ #include -#include #include diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 92d1ca1afe290..ad21766ff09c1 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -60,7 +60,6 @@ #include "dart/dynamics/EllipsoidShape.h" #include "dart/dynamics/CylinderShape.h" #include "dart/dynamics/PlaneShape.h" -#include "dart/dynamics/Shape.h" #include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" @@ -112,9 +111,6 @@ Contact convertContact(const fcl::Contact& fclContact, /// collision algorithm. struct FCLCollisionCallbackData { - /// Collision detector - FCLCollisionDetector* mFclCollisionDetector; - /// FCL collision request fcl::CollisionRequest mFclRequest; @@ -137,15 +133,13 @@ struct FCLCollisionCallbackData /// Constructor FCLCollisionCallbackData( - FCLCollisionDetector* collisionDetector, const Option* option = nullptr, Result* result = nullptr, FCLCollisionDetector::PrimitiveShape_t type = FCLCollisionDetector::MESH, FCLCollisionDetector::ContactPointComputationMethod_t method = FCLCollisionDetector::DART) - : mFclCollisionDetector(collisionDetector), - mOption(option), + : mOption(option), mResult(result), mPrimitiveShapeType(type), mContactPointComputationMethod(method), @@ -785,7 +779,7 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionCallbackData collData( - this, &option, &result, mPrimitiveShapeType, + &option, &result, mPrimitiveShapeType, mContactPointComputationMethod); broadPhaseAlg->collide(&collData, collisionCallback); @@ -824,7 +818,7 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); FCLCollisionCallbackData collData( - this, &option, &result, mPrimitiveShapeType, + &option, &result, mPrimitiveShapeType, mContactPointComputationMethod); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); @@ -902,17 +896,6 @@ FCLCollisionDetector::createCollisionObject( return std::unique_ptr(collObj); } -//============================================================================== -FCLCollisionObject* FCLCollisionDetector::findCollisionObject( - fcl::CollisionObject* fclCollObj) const -{ - auto search = mFCLCollisionObjectMap.find(fclCollObj); - if (mFCLCollisionObjectMap.end() != search) - return search->second; - else - return nullptr; -} - //============================================================================== void FCLCollisionDetector::notifyCollisionObjectDestorying( CollisionObject* collObj) @@ -992,14 +975,19 @@ bool collisionCallback( // Filtering if (filter) { - auto collisionDetector = collData->mFclCollisionDetector; + auto userData1 + = static_cast(o1->getUserData()); + auto userData2 + = static_cast(o2->getUserData()); + assert(userData1); + assert(userData2); - auto collObj1 = collisionDetector->findCollisionObject(o1); - auto collObj2 = collisionDetector->findCollisionObject(o2); - assert(collObj1); - assert(collObj2); + auto collisionObject1 = userData1->mCollisionObject; + auto collisionObject2 = userData2->mCollisionObject; + assert(collisionObject1); + assert(collisionObject2); - if (!filter->needCollision(collObj1, collObj2)) + if (!filter->needCollision(collisionObject2, collisionObject1)) return collData->done; } diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 63772c5cb603f..72a5ce3ccf39f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -121,14 +121,6 @@ class FCLCollisionDetector : public CollisionDetector std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; -public: - - FCLCollisionObject* findCollisionObject( - fcl::CollisionObject* fclCollObj) const; - // TODO(JS): fix const correctness - -protected: - // Documentation inherited void notifyCollisionObjectDestorying(CollisionObject* collObj) override; diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index dee07b9f7d426..cbb6c544990d6 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -38,8 +38,6 @@ #include -#include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionObject.h" #include "dart/collision/fcl/FCLTypes.h" #include "dart/dynamics/SoftMeshShape.h" #include "dart/dynamics/ShapeFrame.h" diff --git a/dart/collision/fcl/FCLCollisionObject.h b/dart/collision/fcl/FCLCollisionObject.h index 57fad845f889a..afc4f066ddfec 100644 --- a/dart/collision/fcl/FCLCollisionObject.h +++ b/dart/collision/fcl/FCLCollisionObject.h @@ -37,11 +37,7 @@ #ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECT_H_ #define DART_COLLISION_FCL_FCLCOLLISIONOBJECT_H_ -#include -#include - #include - #include "dart/collision/CollisionObject.h" namespace dart { From 0888b9c8d349f24daa0439f8a7e5fd70d9ebfc0b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 27 Mar 2016 14:01:17 -0400 Subject: [PATCH 37/67] Implement collision options: binary check, maximum number of contacts --- dart/collision/Option.cpp | 5 +- dart/collision/Option.h | 6 +- .../bullet/BulletCollisionDetector.cpp | 52 ++++++--- dart/collision/dart/DARTCollisionDetector.cpp | 55 ++++++++- dart/collision/fcl/FCLCollisionDetector.cpp | 78 +++++++++---- dart/constraint/ConstraintSolver.cpp | 5 +- unittests/testCollision.cpp | 109 +++++++++++++++--- 7 files changed, 247 insertions(+), 63 deletions(-) diff --git a/dart/collision/Option.cpp b/dart/collision/Option.cpp index 20015569b6d9e..b9289a95386fd 100644 --- a/dart/collision/Option.cpp +++ b/dart/collision/Option.cpp @@ -40,10 +40,13 @@ namespace dart { namespace collision { //============================================================================== -Option::Option(bool enableContact, +Option::Option( + bool enableContact, + bool binaryCheck, size_t maxNumContacts, const std::shared_ptr& collisionFilter) : enableContact(enableContact), + binaryCheck(binaryCheck), maxNumContacts(maxNumContacts), collisionFilter(collisionFilter) { diff --git a/dart/collision/Option.h b/dart/collision/Option.h index a1c4c05edad18..71466b2eef561 100644 --- a/dart/collision/Option.h +++ b/dart/collision/Option.h @@ -52,8 +52,9 @@ struct Option /// simple information whether there is a collision of not. bool enableContact; - // TODO(JS): add option for immediet termination as soon as the first contact - // is detected + /// If true, the collision checking will terminate as soon as the first + /// contact is found. + bool binaryCheck; /// Maximum number of contacts to detect size_t maxNumContacts; @@ -63,6 +64,7 @@ struct Option /// Constructor Option(bool enableContact = true, + bool binaryCheck = false, size_t maxNumContacts = 100, const std::shared_ptr& collisionFilter = nullptr); }; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index cda676665c257..97a8522ebe86f 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -60,8 +60,10 @@ namespace { struct BulletOverlapFilterCallback : public btOverlapFilterCallback { - BulletOverlapFilterCallback(CollisionFilter* filter) - : mFilter(filter) + BulletOverlapFilterCallback(const Option& option, const Result& result) + : mOption(option), + mResult(result), + mDone(false) { // Do nothing } @@ -70,6 +72,16 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override { + if (mDone) + return false; + + if ((mOption.binaryCheck && !mResult.contacts.empty()) + || (mResult.contacts.size() >= mOption.maxNumContacts)) + { + mDone = true; + return false; + } + assert((proxy0 != nullptr && proxy1 != nullptr) && "Bullet broadphase overlapping pair proxies are nullptr"); @@ -78,7 +90,9 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback collide = collide && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - if (collide && mFilter) + const auto& filter = mOption.collisionFilter; + + if (collide && filter) { auto bulletCollObj0 = static_cast(proxy0->m_clientObject); @@ -90,21 +104,26 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback auto userData1 = static_cast( bulletCollObj1->getUserPointer()); - collide = mFilter->needCollision(userData0->collisionObject, - userData1->collisionObject); + collide = filter->needCollision(userData0->collisionObject, + userData1->collisionObject); } return collide; } - CollisionFilter* mFilter; + const Option& mOption; + const Result& mResult; + + /// Whether the collision iteration can stop + mutable bool mDone; }; Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData1, const BulletCollisionObject::UserData* userData2); -void convertContacts(btCollisionWorld* collWorld, Result& result); +void convertContacts( + btCollisionWorld* collWorld, const Option& option, Result& result); } // anonymous namespace @@ -177,13 +196,12 @@ bool BulletCollisionDetector::detect( auto castedData = static_cast(group); auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); - auto filterCallback - = new BulletOverlapFilterCallback(option.collisionFilter.get()); + auto filterCallback = new BulletOverlapFilterCallback(option, result); bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); - convertContacts(bulletCollisionWorld, result); + convertContacts(bulletCollisionWorld, option, result); return !result.contacts.empty(); } @@ -216,13 +234,12 @@ bool BulletCollisionDetector::detect( auto bulletCollisionWorld = group->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); - auto filterCallback - = new BulletOverlapFilterCallback(option.collisionFilter.get()); + auto filterCallback = new BulletOverlapFilterCallback(option, result); bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); - convertContacts(bulletCollisionWorld, result); + convertContacts(bulletCollisionWorld, option, result); return !result.contacts.empty(); } @@ -713,7 +730,8 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, } //============================================================================== -void convertContacts(btCollisionWorld* collWorld, Result& result) +void convertContacts( + btCollisionWorld* collWorld, const Option& option, Result& result) { assert(collWorld); @@ -743,6 +761,12 @@ void convertContacts(btCollisionWorld* collWorld, Result& result) auto& cp = contactManifold->getContactPoint(j); result.contacts.push_back(convertContact(cp, userDataA, userDataB)); + + if (option.binaryCheck) + return; + + if (result.contacts.size() >= option.maxNumContacts) + break; } } } diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 7d209f533109f..e250be978e7a9 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -42,6 +42,7 @@ #include "dart/collision/dart/DARTCollisionObject.h" #include "dart/collision/dart/DARTCollisionGroup.h" #include "dart/dynamics/ShapeFrame.h" +#include "dart/dynamics/EllipsoidShape.h" namespace dart { namespace collision { @@ -49,12 +50,12 @@ namespace collision { namespace { bool checkPair(CollisionObject* o1, CollisionObject* o2, - Result& result); + const Option& option, Result& result); bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, double tol); -void postProcess(CollisionObject* o1, CollisionObject* o2, +void postProcess(CollisionObject* o1, CollisionObject* o2, const Option& option, Result& totalResult, const Result& pairResult); } // anonymous namespace @@ -116,6 +117,7 @@ bool DARTCollisionDetector::detect( if (objects.empty()) return false; + auto done = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects.size() - 1; ++i) @@ -129,8 +131,18 @@ bool DARTCollisionDetector::detect( if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1, collObj2, result); + checkPair(collObj1, collObj2, option, result); + + if ((option.binaryCheck && !result.contacts.empty()) + || (result.contacts.size() >= option.maxNumContacts)) + { + done = true; + break; + } } + + if (done) + break; } return !result.contacts.empty(); @@ -157,6 +169,7 @@ bool DARTCollisionDetector::detect( if (objects1.empty() || objects2.empty()) return false; + auto done = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects1.size(); ++i) @@ -170,8 +183,17 @@ bool DARTCollisionDetector::detect( if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1, collObj2, result); + checkPair(collObj1, collObj2, option, result); + + if (result.contacts.size() >= option.maxNumContacts) + { + done = true; + break; + } } + + if (done) + break; } return !result.contacts.empty(); @@ -189,6 +211,19 @@ void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) if (!shapeFrame) return; + const auto& shape = shapeFrame->getShape(); + + if (shape->getShapeType() == dynamics::Shape::BOX) + return; + + if (shape->getShapeType() == dynamics::Shape::ELLIPSOID) + { + const auto& ellipsoid + = std::static_pointer_cast(shape); + if (ellipsoid->isSphere()) + return; + } + dterr << "[DARTCollisionDetector] Attempting to create shape type '" << shapeFrame->getShape()->getShapeType() << "' that is not supported " << "by DARTCollisionDetector. Currently, only BoxShape and " @@ -228,7 +263,8 @@ void DARTCollisionDetector::notifyCollisionObjectDestorying( namespace { //============================================================================== -bool checkPair(CollisionObject* o1, CollisionObject* o2, Result& result) +bool checkPair(CollisionObject* o1, CollisionObject* o2, + const Option& option, Result& result) { Result pairResult; @@ -237,7 +273,7 @@ bool checkPair(CollisionObject* o1, CollisionObject* o2, Result& result) o2->getShape(), o2->getTransform(), &pairResult.contacts); - postProcess(o1, o2, result, pairResult); + postProcess(o1, o2, option, result, pairResult); return colliding != 0; } @@ -251,6 +287,7 @@ bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, //============================================================================== void postProcess(CollisionObject* o1, CollisionObject* o2, + const Option& option, Result& totalResult, const Result& pairResult) { if (pairResult.contacts.empty()) @@ -278,6 +315,12 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, totalResult.contacts.push_back(pairContact); totalResult.contacts.back().collisionObject1 = o1; totalResult.contacts.back().collisionObject2 = o2; + + if (option.binaryCheck) + break; + + if (totalResult.contacts.size() >= option.maxNumContacts) + break; } } diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index ad21766ff09c1..c8714cd0e75d4 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -73,13 +73,15 @@ bool collisionCallback(fcl::CollisionObject* o1, void* cdata); void postProcessFCL(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - Result& result); + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const Option& option, + Result& result); void postProcessDART(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, + const Option& option, Result& result); int evalContactPosition(const fcl::Contact& fclContact, @@ -118,7 +120,7 @@ struct FCLCollisionCallbackData fcl::CollisionResult mFclResult; /// Collision option of DART - const Option* mOption; + const Option& mOption; /// Collision result of DART Result* mResult; @@ -133,7 +135,7 @@ struct FCLCollisionCallbackData /// Constructor FCLCollisionCallbackData( - const Option* option = nullptr, + const Option& option, Result* result = nullptr, FCLCollisionDetector::PrimitiveShape_t type = FCLCollisionDetector::MESH, @@ -145,8 +147,12 @@ struct FCLCollisionCallbackData mContactPointComputationMethod(method), done(false) { - if (mOption) - convertOption(*mOption, mFclRequest); + convertOption(mOption, mFclRequest); + + mFclRequest.num_max_contacts = std::max(static_cast(100u), + mOption.maxNumContacts); + // Since some contact points can be filtered out in the post process, we ask + // more than the demend. 100 is randomly picked. } }; @@ -779,7 +785,7 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionCallbackData collData( - &option, &result, mPrimitiveShapeType, + option, &result, mPrimitiveShapeType, mContactPointComputationMethod); broadPhaseAlg->collide(&collData, collisionCallback); @@ -963,14 +969,14 @@ bool collisionCallback( { auto collData = static_cast(cdata); + if (collData->done) + return true; + const auto& fclRequest = collData->mFclRequest; auto& fclResult = collData->mFclResult; auto& result = *(collData->mResult); - auto option = collData->mOption; - auto filter = option->collisionFilter; - - if (collData->done) - return true; + const auto& option = collData->mOption; + auto filter = option.collisionFilter; // Filtering if (filter) @@ -997,20 +1003,22 @@ bool collisionCallback( // Perform narrow-phase detection fcl::collide(o1, o2, fclRequest, fclResult); - if (!fclRequest.enable_cost - && (fclResult.isCollision()) - && ((fclResult.numContacts() >= fclRequest.num_max_contacts))) - //|| !collData->checkAllCollisions)) - // TODO(JS): checkAllCollisions should be in FCLCollisionData - { - collData->done = true; - } - if (FCLCollisionDetector::DART == collData->mContactPointComputationMethod && FCLCollisionDetector::MESH == collData->mPrimitiveShapeType) - postProcessDART(fclResult, o1, o2, result); + { + postProcessDART(fclResult, o1, o2, option, result); + } else - postProcessFCL(fclResult, o1, o2, result); + { + postProcessFCL(fclResult, o1, o2, option, result); + } + + if ((option.binaryCheck && !result.contacts.empty()) + || (result.contacts.size() >= option.maxNumContacts)) + { + collData->done = true; + return true; + } return collData->done; } @@ -1019,6 +1027,7 @@ bool collisionCallback( void postProcessFCL(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, + const Option& option, Result& result) { auto numContacts = fclResult.numContacts(); @@ -1083,6 +1092,14 @@ void postProcessFCL(const fcl::CollisionResult& fclResult, } } } + + if (option.binaryCheck) + { + const auto fclContact = fclResult.getContact(i); + convertContact(fclContact, o1, o2); + + return; + } } for (size_t i = 0; i < numContacts; ++i) @@ -1092,6 +1109,9 @@ void postProcessFCL(const fcl::CollisionResult& fclResult, const auto fclContact = fclResult.getContact(i); result.contacts.push_back(convertContact(fclContact, o1, o2)); } + + if (result.contacts.size() >= option.maxNumContacts) + break; } } @@ -1099,6 +1119,7 @@ void postProcessFCL(const fcl::CollisionResult& fclResult, void postProcessDART(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, + const Option& option, Result& result) { auto initNumContacts = fclResult.numContacts(); @@ -1225,12 +1246,21 @@ void postProcessDART(const fcl::CollisionResult& fclResult, } } } + + if (option.binaryCheck) + { + result.contacts.push_back(unfiltered[i]); + return; + } } for (auto i = 0u; i < unfilteredSize; ++i) { if (!markForDeletion[i]) result.contacts.push_back(unfiltered[i]); + + if (result.contacts.size() >= option.maxNumContacts) + break; } } diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 502e40ca07141..9acf704086f8d 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -66,8 +66,9 @@ using namespace dynamics; ConstraintSolver::ConstraintSolver(double timeStep) : mCollisionDetector(collision::FCLCollisionDetector::create()), mCollisionGroup(mCollisionDetector->createCollisionGroup()), - mCollisionOption(collision::Option( - true, 100, std::make_shared())), + mCollisionOption( + collision::Option( + true, false, 100, std::make_shared())), mTimeStep(timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 3185a650e3cf6..46eaab866adb3 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -694,6 +694,87 @@ TEST_F(COLLISION, BoxBox) testBoxBox(dart); } +//============================================================================== +void testOptions(const std::shared_ptr& cd) +{ + auto simpleFrame1 = std::make_shared(Frame::World()); + auto simpleFrame2 = std::make_shared(Frame::World()); + auto simpleFrame3 = std::make_shared(Frame::World()); + + ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); + ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + ShapePtr shape3(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + simpleFrame3->setShape(shape3); + + Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, -0.5); + Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.5, 0.25); + Eigen::Vector3d pos3 = Eigen::Vector3d(0.0, -0.5, 0.25); + simpleFrame1->setTranslation(pos1); + simpleFrame2->setTranslation(pos2); + simpleFrame3->setTranslation(pos3); + + auto group = cd->createCollisionGroup(); + group->registerShapeFrame(simpleFrame1.get()); + group->registerShapeFrame(simpleFrame2.get()); + EXPECT_EQ(group->getNumShapeFrames(), 2u); + + collision::Option option; + collision::Result result; + + result.contacts.clear(); + option.maxNumContacts = 1000u; + option.binaryCheck = false; + EXPECT_TRUE(group->detect(option, result)); + EXPECT_EQ(result.contacts.size(), 4u); + + result.contacts.clear(); + option.maxNumContacts = 2u; + option.binaryCheck = false; + EXPECT_TRUE(group->detect(option, result)); + EXPECT_EQ(result.contacts.size(), 2u); + + group->registerShapeFrame(simpleFrame3.get()); + result.contacts.clear(); + option.maxNumContacts = 1e+3; + option.binaryCheck = true; + EXPECT_TRUE(group->detect(option, result)); + EXPECT_EQ(result.contacts.size(), 1u); +} + +//============================================================================== +TEST_F(COLLISION, Options) +{ + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testOptions(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testOptions(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testOptions(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testOptions(fcl_mesh_fcl); + +#ifdef HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testOptions(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testOptions(dart); +} + //============================================================================== void testBodyNodes(const std::shared_ptr& cd) { @@ -753,17 +834,17 @@ TEST_F(COLLISION, BodyNodeNodes) } //============================================================================== -void testSkeletons(const std::shared_ptr& cd) +void testSkeletons(const std::shared_ptr& /*cd*/) { - Eigen::Vector3d size(1.0, 1.0, 1.0); - Eigen::Vector3d pos1(0.0, 0.0, 0.0); - Eigen::Vector3d pos2(0.5, 0.0, 0.0); +// Eigen::Vector3d size(1.0, 1.0, 1.0); +// Eigen::Vector3d pos1(0.0, 0.0, 0.0); +// Eigen::Vector3d pos2(0.5, 0.0, 0.0); - auto boxSkel1 = createBox(size, pos1); - auto boxSkel2 = createBox(size, pos2); +// auto boxSkel1 = createBox(size, pos1); +// auto boxSkel2 = createBox(size, pos2); - collision::Option option; - collision::Result result; +// collision::Option option; +// collision::Result result; // auto hit = cd->detect(boxSkel1, boxSkel2, option, result); @@ -793,13 +874,13 @@ TEST_F(COLLISION, Skeletons) // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); // testSkeletons(fcl_mesh_fcl); -#ifdef HAVE_BULLET_COLLISION - auto bullet = BulletCollisionDetector::create(); - testSkeletons(bullet); -#endif +//#ifdef HAVE_BULLET_COLLISION +// auto bullet = BulletCollisionDetector::create(); +// testSkeletons(bullet); +//#endif - auto dart = DARTCollisionDetector::create(); - testSkeletons(dart); +// auto dart = DARTCollisionDetector::create(); +// testSkeletons(dart); } //============================================================================== From ce6f5589c190a101c26073ce63c5b9df4b518eab Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 10:32:12 -0400 Subject: [PATCH 38/67] Remove dynamics/CollisionDetector.[h/cpp] --- dart/collision/CollisionFilter.cpp | 56 +++++++++++++ dart/collision/CollisionFilter.h | 14 ++++ dart/constraint/ConstraintSolver.cpp | 5 +- dart/constraint/ContactConstraint.cpp | 2 +- dart/constraint/SoftContactConstraint.cpp | 2 +- dart/dynamics/CollisionDetector.cpp | 97 ----------------------- dart/dynamics/CollisionDetector.h | 65 --------------- 7 files changed, 75 insertions(+), 166 deletions(-) delete mode 100644 dart/dynamics/CollisionDetector.cpp delete mode 100644 dart/dynamics/CollisionDetector.h diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp index 158de3ef67686..90d026a7cd80e 100644 --- a/dart/collision/CollisionFilter.cpp +++ b/dart/collision/CollisionFilter.cpp @@ -36,8 +36,64 @@ #include "dart/collision/CollisionFilter.h" +#include "dart/dynamics/BodyNode.h" +#include "dart/collision/CollisionObject.h" + namespace dart { namespace collision { +//============================================================================== +bool BodyNodeCollisionFilter::needCollision( + const collision::CollisionObject* object1, + const collision::CollisionObject* object2) const +{ + if (object1 == object2) + return false; + + auto shapeNode1 = object1->getShapeFrame()->asShapeNode(); + auto shapeNode2 = object2->getShapeFrame()->asShapeNode(); + + if (!shapeNode1 || !shapeNode2) + return true; + // Assume non-ShapeNode always collides with others. + + auto bodyNode1 = shapeNode1->getBodyNodePtr(); + auto bodyNode2 = shapeNode2->getBodyNodePtr(); + + if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable()) + return false; + + if (bodyNode1->getSkeleton() == bodyNode2->getSkeleton()) + { + auto skeleton = bodyNode1->getSkeleton(); + + if (!skeleton->isEnabledSelfCollisionCheck()) + return false; + + if (!skeleton->isEnabledAdjacentBodyCheck()) + { + if (isAdjacentBodies(bodyNode1, bodyNode2)) + return false; + } + } + + return true; +} + +//============================================================================== +bool BodyNodeCollisionFilter::isAdjacentBodies( + const dynamics::BodyNode* bodyNode1, + const dynamics::BodyNode* bodyNode2) const +{ + if ((bodyNode1->getParentBodyNode() == bodyNode2) + || (bodyNode2->getParentBodyNode() == bodyNode1)) + { + assert(bodyNode1->getSkeleton() == bodyNode2->getSkeleton()); + return true; + } + + return false; +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionFilter.h b/dart/collision/CollisionFilter.h index fbcea6132adac..32d7d87f6c19f 100644 --- a/dart/collision/CollisionFilter.h +++ b/dart/collision/CollisionFilter.h @@ -38,6 +38,11 @@ #define DART_COLLISION_COLLISIONFILTER_H_ namespace dart { + +namespace dynamics { +class BodyNode; +} // namespace dynamics + namespace collision { class CollisionObject; @@ -48,6 +53,15 @@ struct CollisionFilter const CollisionObject* object2) const = 0; }; +struct BodyNodeCollisionFilter : CollisionFilter +{ + bool needCollision(const CollisionObject* object1, + const CollisionObject* object2) const override; + + bool isAdjacentBodies(const dynamics::BodyNode* bodyNode1, + const dynamics::BodyNode* bodyNode2) const; +}; + } // namespace collision } // namespace dart diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 9acf704086f8d..a41eff57a529e 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -37,7 +37,9 @@ #include "dart/constraint/ConstraintSolver.h" #include "dart/common/Console.h" +#include "dart/collision/CollisionObject.h" #include "dart/collision/CollisionGroup.h" +#include "dart/collision/CollisionFilter.h" #include "dart/collision/fcl/FCLCollisionDetector.h" #include "dart/collision/dart/DARTCollisionDetector.h" #ifdef HAVE_BULLET_COLLISION @@ -47,7 +49,6 @@ #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" -#include "dart/dynamics/CollisionDetector.h" #include "dart/constraint/ConstrainedGroup.h" #include "dart/constraint/ContactConstraint.h" #include "dart/constraint/SoftContactConstraint.h" @@ -68,7 +69,7 @@ ConstraintSolver::ConstraintSolver(double timeStep) mCollisionGroup(mCollisionDetector->createCollisionGroup()), mCollisionOption( collision::Option( - true, false, 100, std::make_shared())), + true, false, 100, std::make_shared())), mTimeStep(timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 249bda834e113..140bf7417cc86 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -42,7 +42,7 @@ #include "dart/math/Helpers.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" -#include "dart/dynamics/CollisionDetector.h" +#include "dart/collision/CollisionObject.h" #include "dart/lcpsolver/lcp.h" #define DART_ERROR_ALLOWANCE 0.0 diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index 2c9d0f3d8a7fe..2af46a45b7618 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -45,7 +45,7 @@ #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/Shape.h" -#include "dart/dynamics/CollisionDetector.h" +#include "dart/collision/CollisionObject.h" #include "dart/lcpsolver/lcp.h" #define DART_ERROR_ALLOWANCE 0.0 diff --git a/dart/dynamics/CollisionDetector.cpp b/dart/dynamics/CollisionDetector.cpp deleted file mode 100644 index 14aa1cd5ee395..0000000000000 --- a/dart/dynamics/CollisionDetector.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/dynamics/CollisionDetector.h" - -#include "dart/dynamics/BodyNode.h" - -namespace dart { -namespace dynamics { - -//============================================================================== -bool BodyNodeCollisionFilter::needCollision( - const collision::CollisionObject* object1, - const collision::CollisionObject* object2) const -{ - if (object1 == object2) - return false; - - auto shapeNode1 = object1->getShapeFrame()->asShapeNode(); - auto shapeNode2 = object2->getShapeFrame()->asShapeNode(); - - if (!shapeNode1 || !shapeNode2) - return true; - // Assume non-ShapeNode always collides with others. - - auto bodyNode1 = shapeNode1->getBodyNodePtr(); - auto bodyNode2 = shapeNode2->getBodyNodePtr(); - - if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable()) - return false; - - if (bodyNode1->getSkeleton() == bodyNode2->getSkeleton()) - { - auto skeleton = bodyNode1->getSkeleton(); - - if (!skeleton->isEnabledSelfCollisionCheck()) - return false; - - if (!skeleton->isEnabledAdjacentBodyCheck()) - { - if (isAdjacentBodies(bodyNode1, bodyNode2)) - return false; - } - } - - return true; -} - -//============================================================================== -bool BodyNodeCollisionFilter::isAdjacentBodies(const BodyNode* bodyNode1, - const BodyNode* bodyNode2) const -{ - if ((bodyNode1->getParentBodyNode() == bodyNode2) - || (bodyNode2->getParentBodyNode() == bodyNode1)) - { - assert(bodyNode1->getSkeleton() == bodyNode2->getSkeleton()); - return true; - } - - return false; -} - -} // namespace dynamics -} // namespace dart diff --git a/dart/dynamics/CollisionDetector.h b/dart/dynamics/CollisionDetector.h deleted file mode 100644 index b4e25e439b3e2..0000000000000 --- a/dart/dynamics/CollisionDetector.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_DYNAMICS_COLLISIONDETECTOR_H_ -#define DART_DYNAMICS_COLLISIONDETECTOR_H_ - -#include - -#include "dart/collision/CollisionDetector.h" -#include "dart/collision/CollisionFilter.h" -#include "dart/collision/fcl/FCLCollisionDetector.h" -#include "dart/collision/CollisionObject.h" -#include "dart/dynamics/SmartPointer.h" - -namespace dart { -namespace dynamics { - -struct BodyNodeCollisionFilter : collision::CollisionFilter -{ - bool needCollision(const collision::CollisionObject* object1, - const collision::CollisionObject* object2) const override; - - bool isAdjacentBodies(const BodyNode* bodyNode1, - const BodyNode* bodyNode2) const; -}; - -} // namespace dynamics -} // namespace dart - -#include "dart/dynamics/detail/CollisionDetector.h" - -#endif // DART_DYNAMICS_COLLISIONDETECTOR_H_ From 555d6ff0a009b124bd3d0bf8f3fc5ca2ebf0baf8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 10:47:55 -0400 Subject: [PATCH 39/67] Pass shared_ptr by const reference instead of value --- dart/simulation/World.cpp | 10 +++++----- dart/simulation/World.h | 11 ++++++----- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index f3c0197c80e7f..547ff3f292cd1 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -275,7 +275,7 @@ size_t World::getNumSkeletons() const } //============================================================================== -std::string World::addSkeleton(dynamics::SkeletonPtr _skeleton) +std::string World::addSkeleton(const dynamics::SkeletonPtr& _skeleton) { if(nullptr == _skeleton) { @@ -316,7 +316,7 @@ std::string World::addSkeleton(dynamics::SkeletonPtr _skeleton) } //============================================================================== -void World::removeSkeleton(dynamics::SkeletonPtr _skeleton) +void World::removeSkeleton(const dynamics::SkeletonPtr& _skeleton) { assert(_skeleton != nullptr && "Attempted to remove nullptr Skeleton from world"); @@ -412,7 +412,7 @@ size_t World::getNumSimpleFrames() const } //============================================================================== -std::string World::addSimpleFrame(dynamics::SimpleFramePtr _frame) +std::string World::addSimpleFrame(const dynamics::SimpleFramePtr& _frame) { assert(_frame != nullptr && "Attempted to add nullptr SimpleFrame to world"); @@ -444,7 +444,7 @@ std::string World::addSimpleFrame(dynamics::SimpleFramePtr _frame) } //============================================================================== -void World::removeSimpleFrame(dynamics::SimpleFramePtr _frame) +void World::removeSimpleFrame(const dynamics::SimpleFramePtr& _frame) { assert(_frame != nullptr && "Attempted to remove nullptr SimpleFrame from world"); @@ -540,7 +540,7 @@ Recording* World::getRecording() //============================================================================== void World::handleSkeletonNameChange( - dynamics::ConstMetaSkeletonPtr _skeleton) + const dynamics::ConstMetaSkeletonPtr& _skeleton) { if(nullptr == _skeleton) { diff --git a/dart/simulation/World.h b/dart/simulation/World.h index abfa09958044e..f43b23ba24ea6 100644 --- a/dart/simulation/World.h +++ b/dart/simulation/World.h @@ -136,10 +136,10 @@ class World : public virtual common::Subject size_t getNumSkeletons() const; /// Add a skeleton to this world - std::string addSkeleton(dynamics::SkeletonPtr _skeleton); + std::string addSkeleton(const dynamics::SkeletonPtr& _skeleton); /// Remove a skeleton from this world - void removeSkeleton(dynamics::SkeletonPtr _skeleton); + void removeSkeleton(const dynamics::SkeletonPtr& _skeleton); /// Remove all the skeletons in this world, and return a set of shared /// pointers to them, in case you want to recycle them @@ -158,10 +158,10 @@ class World : public virtual common::Subject size_t getNumSimpleFrames() const; /// Add an Entity to this world - std::string addSimpleFrame(dynamics::SimpleFramePtr _frame); + std::string addSimpleFrame(const dynamics::SimpleFramePtr& _frame); /// Remove a SimpleFrame from this world - void removeSimpleFrame(dynamics::SimpleFramePtr _frame); + void removeSimpleFrame(const dynamics::SimpleFramePtr& _frame); /// Remove all SimpleFrames in this world, and return a set of shared /// pointers to them, in case you want to recycle them @@ -214,7 +214,8 @@ class World : public virtual common::Subject protected: /// Register when a Skeleton's name is changed - void handleSkeletonNameChange(dynamics::ConstMetaSkeletonPtr _skeleton); + void handleSkeletonNameChange( + const dynamics::ConstMetaSkeletonPtr& _skeleton); /// Register when a SimpleFrame's name is changed void handleSimpleFrameNameChange(const dynamics::Entity* _entity); From aac9ac904829636e8a3d190a22219a71a51188f5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 11:25:48 -0400 Subject: [PATCH 40/67] Remove dart/dynamics/detail/CollisionDetector.h --- dart/dynamics/detail/CollisionDetector.h | 48 ------------------------ 1 file changed, 48 deletions(-) delete mode 100644 dart/dynamics/detail/CollisionDetector.h diff --git a/dart/dynamics/detail/CollisionDetector.h b/dart/dynamics/detail/CollisionDetector.h deleted file mode 100644 index 313a626833e4b..0000000000000 --- a/dart/dynamics/detail/CollisionDetector.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (c) 2016, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jeongseok Lee - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef DART_DYNAMICS_DETAIL_COLLISIONDETECTOR_H_ -#define DART_DYNAMICS_DETAIL_COLLISIONDETECTOR_H_ - -#include "dart/collision/CollisionGroup.h" - -namespace dart { -namespace dynamics { - -} // namespace dynamics -} // namespace dart - -#endif // DART_DYNAMICS_COLLISIONDETECTOR_H_ From 54a2316657425572dccc3e590c254d3a7aa00480 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 11:28:38 -0400 Subject: [PATCH 41/67] Fix unresolved conflict in SkelParser.cpp --- dart/utils/SkelParser.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 878687cc80b88..99d29659fe4bd 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -740,13 +740,8 @@ simulation::WorldPtr readWorld( collision::FCLCollisionDetector::DART); } else if (strCD == "dart") -<<<<<<< HEAD collision_detector = collision::DARTCollisionDetector::create(); #if HAVE_BULLET_COLLISION -======= - collision_detector.reset(new collision::DARTCollisionDetector); -#if HAVE_BULLET_COLLISION ->>>>>>> origin/master else if (strCD == "bullet") collision_detector = collision::BulletCollisionDetector::create(); #endif From 5b89458cd812c2fa23877df0317b90c05fb6365e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 12:50:40 -0400 Subject: [PATCH 42/67] Improve API of creating CollisionGroups and registering ShapeFrames to CollisionGroups --- dart/collision/CollisionDetector.cpp | 22 ---- dart/collision/CollisionDetector.h | 25 ++-- dart/collision/CollisionGroup.cpp | 120 ++++++++++++++---- dart/collision/CollisionGroup.h | 66 +++++++--- .../bullet/BulletCollisionDetector.cpp | 17 +-- .../bullet/BulletCollisionDetector.h | 8 -- dart/collision/dart/DARTCollisionDetector.cpp | 14 -- dart/collision/dart/DARTCollisionDetector.h | 8 -- dart/collision/detail/CollisionDetector.h | 62 +++++++++ dart/collision/detail/CollisionGroup.h | 37 ++---- dart/collision/fcl/FCLCollisionDetector.cpp | 14 -- dart/collision/fcl/FCLCollisionDetector.h | 8 -- dart/constraint/ConstraintSolver.cpp | 6 +- unittests/testCollision.cpp | 11 +- 14 files changed, 234 insertions(+), 184 deletions(-) create mode 100644 dart/collision/detail/CollisionDetector.h diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index f7c076390b182..cc417bd2be449 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -48,28 +48,6 @@ namespace dart { namespace collision { -//============================================================================== -std::shared_ptr CollisionDetector::createCollisionGroup( - dynamics::Skeleton* skel) -{ - assert(skel); - - auto group = createCollisionGroup(); - - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - { - auto bodyNode = skel->getBodyNode(i); - auto collisionShapeNodes - = bodyNode->getShapeNodesWith(); - - for (auto& shapeNode : collisionShapeNodes) - group->registerShapeFrame(shapeNode); - } - - return group; -} - //============================================================================== CollisionObject* CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 691b19bbf80ea..6f045a0eeb6f0 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -67,17 +67,12 @@ class CollisionDetector : public std::enable_shared_from_this /// Create a collision group virtual std::shared_ptr createCollisionGroup() = 0; - /// Create a collision group from ShapeFrame - virtual std::shared_ptr createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) = 0; - - /// Create a collision group from ShapeFrames - virtual std::shared_ptr createCollisionGroup( - const std::vector& shapeFrames) = 0; - - /// Create a collision group from Skeleton - std::shared_ptr createCollisionGroup( - dynamics::Skeleton* skeleton); + /// Create a collision group from any object that is supported by + /// CollisionGroup::registerShapeFramesOf(). Currently, the supporting objects + /// are ShapeFrame, std::vector, CollisionGroup, BodyNode, and + /// Skeleton. + template + std::shared_ptr createCollisionGroup(const Args&... args); /// Perform collision detection for group. virtual bool detect(CollisionGroup* group, @@ -103,9 +98,9 @@ class CollisionDetector : public std::enable_shared_from_this CollisionObject* claimCollisionObject(const dynamics::ShapeFrame* shapeFrame); // TODO(JS): Maybe WeakShapeFramePtr - /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject - /// will be destroyed if no CollisionGroup holds it. - void reclaimCollisionObject(const CollisionObject* shapeFrame); + /// Reclaim a CollisionObject. The CollisionObject will be destroyed if no + /// CollisionGroup holds it. + void reclaimCollisionObject(const CollisionObject* collObj); /// Create CollisionObject virtual std::unique_ptr createCollisionObject( @@ -202,4 +197,6 @@ class CollisionDetector::RefCountingCollisionObjectManager : } // namespace collision } // namespace dart +#include "dart/collision/detail/CollisionDetector.h" + #endif // DART_COLLISION_COLLISIONDETECTOR_H_ diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 571a744d61393..3ceb015c508e5 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -92,30 +92,62 @@ void CollisionGroup::registerShapeFrames( } //============================================================================== -void CollisionGroup::registerShapeFramesFrom(const dynamics::Skeleton* skel) +void CollisionGroup::registerShapeFrames() { - assert(skel); + // Do nothing +} - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - { - auto bodyNode = skel->getBodyNode(i); - auto collisionShapeNodes - = bodyNode->getShapeNodesWith(); +//============================================================================== +void CollisionGroup::registerShapeFramesOf( + const dynamics::ShapeFrame* shapeFrame) +{ + registerShapeFrame(shapeFrame); +} - for (auto& shapeNode : collisionShapeNodes) - registerShapeFrame(shapeNode); +//============================================================================== +void CollisionGroup::registerShapeFramesOf( + const std::vector& shapeFrames) +{ + registerShapeFrames(shapeFrames); +} + +//============================================================================== +void CollisionGroup::registerShapeFramesOf(const CollisionGroup* other) +{ + assert(other); + + if (other && this != other) + { + for (const auto& shapeFrame : other->mShapeFrames) + registerShapeFrame(shapeFrame); } } //============================================================================== -void CollisionGroup::registerShapeFramesFrom() +void CollisionGroup::registerShapeFramesOf(const dynamics::BodyNode* bodyNode) { - // Do nothing + assert(bodyNode); + + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); + + for (auto& shapeNode : collisionShapeNodes) + registerShapeFrame(shapeNode); +} + +//============================================================================== +void CollisionGroup::registerShapeFramesOf(const dynamics::Skeleton* skel) +{ + assert(skel); + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + registerShapeFramesOf(skel->getBodyNode(i)); } //============================================================================== -void CollisionGroup::unregisterShapeFrame(const dynamics::ShapeFrame* shapeFrame) +void CollisionGroup::unregisterShapeFrame( + const dynamics::ShapeFrame* shapeFrame) { if (!shapeFrame) return; @@ -147,26 +179,64 @@ void CollisionGroup::unregisterShapeFrames( } //============================================================================== -void CollisionGroup::unregisterShapeFramesFrom(const dynamics::Skeleton* skel) +void CollisionGroup::unregisterShapeFramesOf() { - assert(skel); + // Do nothing +} - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - { - auto bodyNode = skel->getBodyNode(i); - auto collisionShapeNodes - = bodyNode->getShapeNodesWith(); +//============================================================================== +void CollisionGroup::unregisterShapeFramesOf( + const dynamics::ShapeFrame* shapeFrame) +{ + unregisterShapeFrame(shapeFrame); +} - for (auto& shapeNode : collisionShapeNodes) - unregisterShapeFrame(shapeNode); +//============================================================================== +void CollisionGroup::unregisterShapeFramesOf( + const std::vector& shapeFrames) +{ + unregisterShapeFrames(shapeFrames); +} + +//============================================================================== +void CollisionGroup::unregisterShapeFramesOf(const CollisionGroup* other) +{ + assert(other); + + if (other) + { + if (this == other) + { + unregisterAllShapeFrames(); + return; + } + + for (const auto& shapeFrame : other->mShapeFrames) + unregisterShapeFrame(shapeFrame); } } //============================================================================== -void CollisionGroup::unregisterShapeFramesFrom() +void CollisionGroup::unregisterShapeFramesOf( + const dynamics::BodyNode* bodyNode) { - // Do nothing + assert(bodyNode); + + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); + + for (auto& shapeNode : collisionShapeNodes) + unregisterShapeFrame(shapeNode); +} + +//============================================================================== +void CollisionGroup::unregisterShapeFramesOf(const dynamics::Skeleton* skel) +{ + assert(skel); + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + unregisterShapeFramesOf(skel->getBodyNode(i)); } //============================================================================== diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 216b5b5373427..c8db421f5d0ce 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -66,17 +66,33 @@ class CollisionGroup void registerShapeFrames( const std::vector& shapeFrames); - /// Register all the ShapeFrames with CollisionAddon to this CollisionGroup - void registerShapeFramesFrom(const dynamics::Skeleton* skeleton); - - /// Register all the ShapeFrames in the other groups to this CollisionGroup - template - void registerShapeFramesFrom(const CollisionGroup* first, - const Others*... others); + /// Register all the ShapeFrames in the other CollisionGroups to this + /// CollisionGroup + template + void registerShapeFrames(const First* first, const Others*... others); /// Do nothing. This function is for terminating the variadic template - /// function template registerShapeFramesFrom(). - void registerShapeFramesFrom(); + /// function template registerShapeFrames(). + void registerShapeFrames(); + + /// As as registerShapeFrame() + void registerShapeFramesOf(const dynamics::ShapeFrame* shapeFrame); + + /// As as registerShapeFrames() + void registerShapeFramesOf( + const std::vector& shapeFrames); + + /// Register all the ShapeFrames in the other CollisionGroup to this + /// CollisionGroup + void registerShapeFramesOf(const CollisionGroup* other); + + /// Register all the BodyNode's ShapeFrames having CollisionAddon to this + /// CollisionGroup + void registerShapeFramesOf(const dynamics::BodyNode* bodyNode); + + /// Register all the Skeleton's ShapeFrames having CollisionAddon to this + /// CollisionGroup + void registerShapeFramesOf(const dynamics::Skeleton* skeleton); /// Unregister a ShapeFrame from this CollisionGroup void unregisterShapeFrame(const dynamics::ShapeFrame* shapeFrame); @@ -85,19 +101,33 @@ class CollisionGroup void unregisterShapeFrames( const std::vector& shapeFrames); - /// Unregister all the ShapeFrames with CollisionAddon from this + /// Unregister all the ShapeFrames in the other CollisionGroups from this + /// CollisionGroup + template + void unregisterShapeFrames(const First* first, const Others*... others); + + /// Do nothing. This function is for terminating the variadic template + /// function template unregisterShapeFrames(). + void unregisterShapeFramesOf(); + + /// As as unregisterShapeFrame() + void unregisterShapeFramesOf(const dynamics::ShapeFrame* shapeFrame); + + /// As as unregisterShapeFrames() + void unregisterShapeFramesOf( + const std::vector& shapeFrames); + + /// Unregister all the ShapeFrames in the other CollisionGroup from this /// CollisionGroup - void unregisterShapeFramesFrom(const dynamics::Skeleton* skeleton); + void unregisterShapeFramesOf(const CollisionGroup* other); - /// Unregister all the ShapeFrames in the other groups from this + /// Unregister all the BodyNode's ShapeFrames having CollisionAddon from this /// CollisionGroup - template - void unregisterShapeFramesFrom(const CollisionGroup* first, - const Others*... others); + void unregisterShapeFramesOf(const dynamics::BodyNode* bodyNode); - /// Do nothing. This function is for terminating the variadic template - /// function template unregisterShapeFramesFrom(). - void unregisterShapeFramesFrom(); + /// Unregister all the Skeleton's ShapeFrames having CollisionAddon from this + /// CollisionGroup + void unregisterShapeFramesOf(const dynamics::Skeleton* skeleton); /// Unregister all the ShapeFrames in this CollisionGroup void unregisterAllShapeFrames(); diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 97a8522ebe86f..97dbe90fe94ac 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -161,21 +161,6 @@ std::shared_ptr BulletCollisionDetector::createCollisionGroup() return std::make_shared(shared_from_this()); } -//============================================================================== -std::shared_ptr BulletCollisionDetector::createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) -{ - return std::make_shared(shared_from_this(), shapeFrame); -} - -//============================================================================== -std::shared_ptr BulletCollisionDetector::createCollisionGroup( - const std::vector& shapeFrames) -{ - return std::make_shared(shared_from_this(), - shapeFrames); -} - //============================================================================== bool BulletCollisionDetector::detect( CollisionGroup* group, const Option& option, Result& result) @@ -229,7 +214,7 @@ bool BulletCollisionDetector::detect( } auto group = common::make_unique(shared_from_this()); - group->registerShapeFramesFrom(group1, group2); + group->registerShapeFrames(group1, group2); group->update(); auto bulletCollisionWorld = group->getBulletCollisionWorld(); diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 9f55a331b3f6e..367300371d4c6 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -67,14 +67,6 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited std::shared_ptr createCollisionGroup() override; - // Documentation inherited - std::shared_ptr createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) override; - - // Documentation inherited - std::shared_ptr createCollisionGroup( - const std::vector& shapeFrames) override; - // Documentation inherited bool detect(CollisionGroup* group, const Option& option, Result& result) override; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index e250be978e7a9..0934bf7771004 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -87,20 +87,6 @@ std::shared_ptr DARTCollisionDetector::createCollisionGroup() return std::make_shared(shared_from_this()); } -//============================================================================== -std::shared_ptr DARTCollisionDetector::createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) -{ - return std::make_shared(shared_from_this(), shapeFrame); -} - -//============================================================================== -std::shared_ptr DARTCollisionDetector::createCollisionGroup( - const std::vector& shapeFrames) -{ - return std::make_shared(shared_from_this(), shapeFrames); -} - //============================================================================== bool DARTCollisionDetector::detect( CollisionGroup* group, diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 75eb8999fdad2..00c2aead345ca 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -60,14 +60,6 @@ class DARTCollisionDetector : public CollisionDetector // Documentation inherited std::shared_ptr createCollisionGroup() override; - // Documentation inherited - std::shared_ptr createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) override; - - // Documentation inherited - std::shared_ptr createCollisionGroup( - const std::vector& shapeFrames) override; - // Documentation inherited bool detect(CollisionGroup* group, const Option& option, Result& result) override; diff --git a/dart/collision/detail/CollisionDetector.h b/dart/collision/detail/CollisionDetector.h new file mode 100644 index 0000000000000..3588b72214ee6 --- /dev/null +++ b/dart/collision/detail/CollisionDetector.h @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ +#define DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ + +#include "dart/collision/CollisionDetector.h" + +#include "dart/collision/CollisionGroup.h" + +namespace dart { +namespace collision { + +//============================================================================== +template +std::shared_ptr CollisionDetector::createCollisionGroup( + const Args&... args) +{ + auto group = createCollisionGroup(); + + group->registerShapeFrames(args...); + + return group; +} + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_DETAIL_COLLISIONDETECTOR_H_ diff --git a/dart/collision/detail/CollisionGroup.h b/dart/collision/detail/CollisionGroup.h index beda5ef3b2a39..1fcd5bd4d6e70 100644 --- a/dart/collision/detail/CollisionGroup.h +++ b/dart/collision/detail/CollisionGroup.h @@ -38,43 +38,26 @@ #define DART_COLLISION_DETAIL_COLLISIONGROUP_H_ #include "dart/collision/CollisionGroup.h" -#include "dart/common/Console.h" namespace dart { namespace collision { //============================================================================== -template -void CollisionGroup::registerShapeFramesFrom(const CollisionGroup* first, - const Others*... others) +template +void CollisionGroup::registerShapeFrames( + const First* first, const Others*... others) { - if (first && this != first) - { - for (const auto& shapeFrame : first->mShapeFrames) - registerShapeFrame(shapeFrame); - } - - registerShapeFramesFrom(others...); + registerShapeFramesOf(first); + registerShapeFrames(others...); } //============================================================================== -template -void CollisionGroup::unregisterShapeFramesFrom(const CollisionGroup* first, - const Others*... others) +template +void CollisionGroup::unregisterShapeFrames( + const First* first, const Others*... others) { - if (first) - { - if (this == first) - { - unregisterAllShapeFrames(); - return; - } - - for (const auto& shapeFrame : first->mShapeFrames) - unregisterShapeFrame(shapeFrame); - } - - unregisterShapeFramesFrom(others...); + unregisterShapeFramesOf(first); + unregisterShapeFrames(others...); } } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index c8714cd0e75d4..d2e468b1a24cf 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -750,20 +750,6 @@ std::shared_ptr FCLCollisionDetector::createCollisionGroup() return std::make_shared(shared_from_this()); } -//============================================================================== -std::shared_ptr FCLCollisionDetector::createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) -{ - return std::make_shared(shared_from_this(), shapeFrame); -} - -//============================================================================== -std::shared_ptr FCLCollisionDetector::createCollisionGroup( - const std::vector& shapeFrames) -{ - return std::make_shared(shared_from_this(), shapeFrames); -} - //============================================================================== bool FCLCollisionDetector::detect( CollisionGroup* group, const Option& option, Result& result) diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 72a5ce3ccf39f..215ff4f318394 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -65,14 +65,6 @@ class FCLCollisionDetector : public CollisionDetector // Documentation inherited std::shared_ptr createCollisionGroup() override; - // Documentation inherited - std::shared_ptr createCollisionGroup( - const dynamics::ShapeFrame* shapeFrame) override; - - // Documentation inherited - std::shared_ptr createCollisionGroup( - const std::vector& shapeFrames) override; - // Documentation inherited bool detect(CollisionGroup* group, const Option& option, Result& result) override; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index fbe5ac5a4d1ad..8e0679bed19d6 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -99,7 +99,7 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) if (!containSkeleton(skeleton)) { auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->registerShapeFramesFrom(group.get()); + mCollisionGroup->registerShapeFramesOf(group.get()); mSkeletons.push_back(skeleton); mConstrainedGroups.reserve(mSkeletons.size()); @@ -128,7 +128,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) if (containSkeleton(skeleton)) { auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->unregisterShapeFramesFrom(group.get()); + mCollisionGroup->unregisterShapeFramesOf(group.get()); mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), mSkeletons.end()); @@ -231,7 +231,7 @@ void ConstraintSolver::setCollisionDetector( auto newCollisionGroup = mCollisionDetector->createCollisionGroup(); for (const auto& skeleton : mSkeletons) - newCollisionGroup->registerShapeFramesFrom(skeleton.get()); + newCollisionGroup->registerShapeFramesOf(skeleton.get()); mCollisionGroup = newCollisionGroup; } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 162c3ef3991ba..e4fa281734226 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -530,8 +530,8 @@ void testSimpleFrames(const std::shared_ptr& cd) auto group2 = cd->createCollisionGroup(simpleFrame2.get()); auto group3 = cd->createCollisionGroup(simpleFrame3.get()); - auto groupAll = cd->createCollisionGroup(); - groupAll->registerShapeFramesFrom(group1.get(), group2.get(), group3.get()); + auto groupAll = cd->createCollisionGroup( + group1.get(), group2.get(), group3.get()); EXPECT_EQ(group1->getNumShapeFrames(), 1u); EXPECT_EQ(group2->getNumShapeFrames(), 1u); @@ -625,8 +625,7 @@ void testBoxBox(const std::shared_ptr& cd, auto group1 = cd->createCollisionGroup(simpleFrame1.get()); auto group2 = cd->createCollisionGroup(simpleFrame2.get()); - auto groupAll = cd->createCollisionGroup(); - groupAll->registerShapeFramesFrom(group1.get(), group2.get()); + auto groupAll = cd->createCollisionGroup(group1.get(), group2.get()); EXPECT_EQ(group1->getNumShapeFrames(), 1u); EXPECT_EQ(group2->getNumShapeFrames(), 1u); @@ -715,9 +714,7 @@ void testOptions(const std::shared_ptr& cd) simpleFrame2->setTranslation(pos2); simpleFrame3->setTranslation(pos3); - auto group = cd->createCollisionGroup(); - group->registerShapeFrame(simpleFrame1.get()); - group->registerShapeFrame(simpleFrame2.get()); + auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get()); EXPECT_EQ(group->getNumShapeFrames(), 2u); collision::Option option; From 4993f060adb7d7cb430e4e195bdde78d7c67f306 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 14:59:04 -0400 Subject: [PATCH 43/67] Improve Result class --- dart/collision/CollisionDetector.h | 2 +- dart/collision/Result.cpp | 46 ++++++++++++++++++ dart/collision/Result.h | 30 ++++++++++-- .../bullet/BulletCollisionDetector.cpp | 16 +++---- dart/collision/dart/DARTCollide.cpp | 48 +++++++++---------- dart/collision/dart/DARTCollide.h | 22 ++++----- dart/collision/dart/DARTCollisionDetector.cpp | 31 ++++++------ dart/collision/fcl/FCLCollisionDetector.cpp | 22 ++++----- dart/constraint/ConstraintSolver.cpp | 6 +-- dart/gui/SimWindow.cpp | 7 ++- dart/simulation/World.cpp | 6 +-- unittests/testCollision.cpp | 17 ++++--- unittests/testConstraint.cpp | 4 +- 13 files changed, 162 insertions(+), 95 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 6f045a0eeb6f0..dbea9fa936a8b 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -67,7 +67,7 @@ class CollisionDetector : public std::enable_shared_from_this /// Create a collision group virtual std::shared_ptr createCollisionGroup() = 0; - /// Create a collision group from any object that is supported by + /// Create a collision group from any objects that are supported by /// CollisionGroup::registerShapeFramesOf(). Currently, the supporting objects /// are ShapeFrame, std::vector, CollisionGroup, BodyNode, and /// Skeleton. diff --git a/dart/collision/Result.cpp b/dart/collision/Result.cpp index 65a29807a5aee..d01fc759a70c1 100644 --- a/dart/collision/Result.cpp +++ b/dart/collision/Result.cpp @@ -39,5 +39,51 @@ namespace dart { namespace collision { +//============================================================================== +void Result::addContact(const Contact& contact) +{ + mContacts.push_back(contact); +} + +//============================================================================== +size_t Result::getNumContacts() const +{ + return mContacts.size(); +} + +//============================================================================== +Contact& Result::getContact(size_t index) +{ + assert(index < mContacts.size()); + + return mContacts[index]; +} + +//============================================================================== +const Contact& Result::getContact(size_t index) const +{ + assert(index < mContacts.size()); + + return mContacts[index]; +} + +//============================================================================== +const std::vector& Result::getContacts() const +{ + return mContacts; +} + +//============================================================================== +bool Result::isCollision() const +{ + return !mContacts.empty(); +} + +//============================================================================== +void Result::clear() +{ + mContacts.clear(); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/Result.h b/dart/collision/Result.h index cdaeb3ad660a1..544d13d4b0576 100644 --- a/dart/collision/Result.h +++ b/dart/collision/Result.h @@ -43,12 +43,36 @@ namespace dart { namespace collision { -struct Result +class Result { +public: + + /// Add one contact + void addContact(const Contact& contact); + + /// Return number of contacts + size_t getNumContacts() const; + + /// Return the index-th contact + Contact& getContact(size_t index); + + /// Return (const) the index-th contact + const Contact& getContact(size_t index) const; + + /// Return contacts + const std::vector& getContacts() const; + + /// Return binary collision result + bool isCollision() const; + + /// Clear all the contacts + void clear(); + +protected: + /// List of contact information for each contact - std::vector contacts; + std::vector mContacts; - size_t getNumContacts() const { return contacts.size(); } }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 97dbe90fe94ac..59deaff3d0ea1 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -75,8 +75,8 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback if (mDone) return false; - if ((mOption.binaryCheck && !mResult.contacts.empty()) - || (mResult.contacts.size() >= mOption.maxNumContacts)) + if ((mOption.binaryCheck && mResult.isCollision()) + || (mResult.getNumContacts() >= mOption.maxNumContacts)) { mDone = true; return false; @@ -165,7 +165,7 @@ std::shared_ptr BulletCollisionDetector::createCollisionGroup() bool BulletCollisionDetector::detect( CollisionGroup* group, const Option& option, Result& result) { - result.contacts.clear(); + result.clear(); if (!group) return false; @@ -188,7 +188,7 @@ bool BulletCollisionDetector::detect( convertContacts(bulletCollisionWorld, option, result); - return !result.contacts.empty(); + return result.isCollision(); } //============================================================================== @@ -196,7 +196,7 @@ bool BulletCollisionDetector::detect( CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { - result.contacts.clear(); + result.clear(); if (!group1 || !group2) return false; @@ -226,7 +226,7 @@ bool BulletCollisionDetector::detect( convertContacts(bulletCollisionWorld, option, result); - return !result.contacts.empty(); + return result.isCollision(); } //============================================================================== @@ -745,12 +745,12 @@ void convertContacts( { auto& cp = contactManifold->getContactPoint(j); - result.contacts.push_back(convertContact(cp, userDataA, userDataB)); + result.addContact(convertContact(cp, userDataA, userDataB)); if (option.binaryCheck) return; - if (result.contacts.size() >= option.maxNumContacts) + if (result.getNumContacts() >= option.maxNumContacts) break; } } diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 99e06f8ae5a6a..378849a882b7c 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -419,7 +419,7 @@ void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, // fields. int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, const dVector3 p2, const dMatrix3 R2, const dVector3 side2, - std::vector& result) + Result& result) { const double fudge_factor = 1.05; dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0}; @@ -586,7 +586,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, contact.point = point_vec; contact.normal = normal; contact.penetrationDepth = penetration; - result.push_back(contact); + result.addContact(contact); } return 1; } @@ -767,7 +767,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, contact.point = point_vec; contact.normal = normal; contact.penetrationDepth = dep[j]; - result.push_back(contact); + result.addContact(contact); } } else { @@ -794,7 +794,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, contact.point = point_vec; contact.normal = normal; contact.penetrationDepth = dep[iret[j]]; - result.push_back(contact); + result.addContact(contact); } } return cnum; @@ -802,7 +802,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - std::vector* result) + Result& result) { dVector3 halfSize0; dVector3 halfSize1; @@ -821,12 +821,12 @@ int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, convVector(T0.translation(), p0); convVector(T1.translation(), p1); - return dBoxBox(p1, R1, halfSize1, p0, R0, halfSize0, *result); + return dBoxBox(p1, R1, halfSize1, p0, R0, halfSize0, result); } int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const double& r1, const Eigen::Isometry3d& T1, - std::vector* result) + Result& result) { Eigen::Vector3d halfSize = 0.5 * size0; bool inside_box = true; @@ -877,7 +877,7 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, contact.point = c0; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } @@ -900,7 +900,7 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); } else { @@ -928,14 +928,14 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); } return 1; } int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - std::vector* result) + Result& result) { Eigen::Vector3d size = 0.5 * size1; bool inside_box = true; @@ -984,7 +984,7 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, contact.point = c0; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } @@ -1007,7 +1007,7 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact);} + result.addContact(contact);} else { double min = size[0] - std::abs(p[0]); @@ -1033,14 +1033,14 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); } return 1; } int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, const double& _r1, const Eigen::Isometry3d& c1, - std::vector* result) + Result& result) { double r0 = _r0; double r1 = _r1; @@ -1068,7 +1068,7 @@ int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } @@ -1080,14 +1080,14 @@ int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } int collideCylinderSphere(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const double& sphere_rad, const Eigen::Isometry3d& T1, - std::vector* result) + Result& result) { Eigen::Vector3d center = T0.inverse() * T1.translation(); @@ -1099,7 +1099,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons contact.penetrationDepth = 0.5 * (half_height + sphere_rad - math::sign(center[2]) * center[2]); contact.point = T0 * Eigen::Vector3d(center[0], center[1], half_height - contact.penetrationDepth); contact.normal = T0.linear() * Eigen::Vector3d(0.0, 0.0, math::sign(center[2])); - result->push_back(contact); + result.addContact(contact); return 1; } else @@ -1123,7 +1123,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } } @@ -1139,7 +1139,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } } @@ -1149,7 +1149,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1, - std::vector* result) + Result& result) { Eigen::Vector3d normal = T1.linear() * plane_normal; Eigen::Vector3d Rx = T0.linear().rightCols(1); @@ -1209,7 +1209,7 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; - result->push_back(contact); + result.addContact(contact); return 1; } @@ -1219,7 +1219,7 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const //============================================================================== int collide(dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, dynamics::ConstShapePtr shape1, const Eigen::Isometry3d& T1, - std::vector* result) + Result& result) { dynamics::Shape::ShapeType LeftType = shape0->getShapeType(); dynamics::Shape::ShapeType RightType = shape1->getShapeType(); diff --git a/dart/collision/dart/DARTCollide.h b/dart/collision/dart/DARTCollide.h index cbde38d8016f9..07887dbb66e8a 100644 --- a/dart/collision/dart/DARTCollide.h +++ b/dart/collision/dart/DARTCollide.h @@ -53,37 +53,37 @@ class Shape; namespace dart { namespace collision { -int collide(dart::dynamics::ConstShapePtr _shape0, const Eigen::Isometry3d& _T0, - dart::dynamics::ConstShapePtr _shape1, const Eigen::Isometry3d& _T1, - std::vector* _result); +int collide(dart::dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, + dart::dynamics::ConstShapePtr shape1, const Eigen::Isometry3d& T1, + Result& result); int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - std::vector* result); + Result& result); int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const double& r1, const Eigen::Isometry3d& T1, - std::vector* result); + Result& result); int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - std::vector* result); + Result& result); -int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, - const double& _r1, const Eigen::Isometry3d& c1, - std::vector* result); +int collideSphereSphere(const double& r0, const Eigen::Isometry3d& c0, + const double& r1, const Eigen::Isometry3d& c1, + Result& result); int collideCylinderSphere( const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const double& sphere_rad, const Eigen::Isometry3d& T1, - std::vector* result); + Result& result); int collideCylinderPlane( const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1, - std::vector* result); + Result& result); } // namespace collision } // namespace dart diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 0934bf7771004..15de42c61bf50 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -92,7 +92,7 @@ bool DARTCollisionDetector::detect( CollisionGroup* group, const Option& option, Result& result) { - result.contacts.clear(); + result.clear(); assert(group); assert(group->getCollisionDetector()->getType() @@ -119,8 +119,8 @@ bool DARTCollisionDetector::detect( checkPair(collObj1, collObj2, option, result); - if ((option.binaryCheck && !result.contacts.empty()) - || (result.contacts.size() >= option.maxNumContacts)) + if ((option.binaryCheck && result.isCollision()) + || (result.getNumContacts() >= option.maxNumContacts)) { done = true; break; @@ -131,7 +131,7 @@ bool DARTCollisionDetector::detect( break; } - return !result.contacts.empty(); + return result.isCollision(); } //============================================================================== @@ -140,7 +140,7 @@ bool DARTCollisionDetector::detect( CollisionGroup* group2, const Option& option, Result& result) { - result.contacts.clear(); + result.clear(); assert(group1); assert(group2); @@ -171,7 +171,7 @@ bool DARTCollisionDetector::detect( checkPair(collObj1, collObj2, option, result); - if (result.contacts.size() >= option.maxNumContacts) + if (result.getNumContacts() >= option.maxNumContacts) { done = true; break; @@ -182,7 +182,7 @@ bool DARTCollisionDetector::detect( break; } - return !result.contacts.empty(); + return result.isCollision(); } //============================================================================== @@ -257,7 +257,7 @@ bool checkPair(CollisionObject* o1, CollisionObject* o2, // Perform narrow-phase detection auto colliding = collide(o1->getShape(), o1->getTransform(), o2->getShape(), o2->getTransform(), - &pairResult.contacts); + pairResult); postProcess(o1, o2, option, result, pairResult); @@ -276,17 +276,17 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, const Option& option, Result& totalResult, const Result& pairResult) { - if (pairResult.contacts.empty()) + if (!pairResult.isCollision()) return; // Don't add repeated points const auto tol = 3.0e-12; - for (auto pairContact : pairResult.contacts) + for (auto pairContact : pairResult.getContacts()) { auto foundClose = false; - for (auto totalContact : totalResult.contacts) + for (auto totalContact : totalResult.getContacts()) { if (isClose(pairContact.point, totalContact.point, tol)) { @@ -298,14 +298,15 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, if (foundClose) continue; - totalResult.contacts.push_back(pairContact); - totalResult.contacts.back().collisionObject1 = o1; - totalResult.contacts.back().collisionObject2 = o2; + auto contact = pairContact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; + totalResult.addContact(contact); if (option.binaryCheck) break; - if (totalResult.contacts.size() >= option.maxNumContacts) + if (totalResult.getNumContacts() >= option.maxNumContacts) break; } } diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index d2e468b1a24cf..d139d798b5028 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -754,7 +754,7 @@ std::shared_ptr FCLCollisionDetector::createCollisionGroup() bool FCLCollisionDetector::detect( CollisionGroup* group, const Option& option, Result& result) { - result.contacts.clear(); + result.clear(); if (!group) return false; @@ -775,7 +775,7 @@ bool FCLCollisionDetector::detect( mContactPointComputationMethod); broadPhaseAlg->collide(&collData, collisionCallback); - return !result.contacts.empty(); + return result.isCollision(); } //============================================================================== @@ -783,7 +783,7 @@ bool FCLCollisionDetector::detect( CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { - result.contacts.clear(); + result.clear(); if (!group1 || !group2) return false; @@ -814,7 +814,7 @@ bool FCLCollisionDetector::detect( mContactPointComputationMethod); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); - return !result.contacts.empty(); + return result.isCollision(); } //============================================================================== @@ -999,8 +999,8 @@ bool collisionCallback( postProcessFCL(fclResult, o1, o2, option, result); } - if ((option.binaryCheck && !result.contacts.empty()) - || (result.contacts.size() >= option.maxNumContacts)) + if ((option.binaryCheck && result.isCollision()) + || (result.getNumContacts() >= option.maxNumContacts)) { collData->done = true; return true; @@ -1093,10 +1093,10 @@ void postProcessFCL(const fcl::CollisionResult& fclResult, if (!markForDeletion[i]) { const auto fclContact = fclResult.getContact(i); - result.contacts.push_back(convertContact(fclContact, o1, o2)); + result.addContact(convertContact(fclContact, o1, o2)); } - if (result.contacts.size() >= option.maxNumContacts) + if (result.getNumContacts() >= option.maxNumContacts) break; } } @@ -1235,7 +1235,7 @@ void postProcessDART(const fcl::CollisionResult& fclResult, if (option.binaryCheck) { - result.contacts.push_back(unfiltered[i]); + result.addContact(unfiltered[i]); return; } } @@ -1243,9 +1243,9 @@ void postProcessDART(const fcl::CollisionResult& fclResult, for (auto i = 0u; i < unfilteredSize; ++i) { if (!markForDeletion[i]) - result.contacts.push_back(unfiltered[i]); + result.addContact(unfiltered[i]); - if (result.contacts.size() >= option.maxNumContacts) + if (result.getNumContacts() >= option.maxNumContacts) break; } } diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 8e0679bed19d6..24f58c3491844 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -379,7 +379,7 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- // Update automatic constraints: contact constraints //---------------------------------------------------------------------------- - mCollisionResult.contacts.clear(); + mCollisionResult.clear(); mCollisionGroup->detect(mCollisionOption, mCollisionResult); @@ -390,9 +390,9 @@ void ConstraintSolver::updateConstraints() mSoftContactConstraints.clear(); // Create new contact constraints - for (auto i = 0u; i < mCollisionResult.contacts.size(); ++i) + for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i) { - collision::Contact& ct = mCollisionResult.contacts[i]; + auto& ct = mCollisionResult.getContact(i); if (isSoftContact(ct)) { diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index fd57724ff6443..f22d436c85414 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -149,10 +149,9 @@ void SimWindow::draw() { if (mShowMarkers) { const auto result = mWorld->getConstraintSolver()->getLastCollisionResult(); - const auto& contacts = result.contacts; - for (size_t k = 0; k < contacts.size(); k++) { - Eigen::Vector3d v = contacts[k].point; - Eigen::Vector3d f = contacts[k].force / 10.0; + for (const auto& contact : result.getContacts()) { + Eigen::Vector3d v = contact.point; + Eigen::Vector3d f = contact.force / 10.0; glBegin(GL_LINES); glVertex3f(v[0], v[1], v[2]); glVertex3f(v[0] + f[0], v[1] + f[1], v[2] + f[2]); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 547ff3f292cd1..4e97faa7ba4ae 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -512,7 +512,7 @@ constraint::ConstraintSolver* World::getConstraintSolver() const void World::bake() { const auto collisionResult = getConstraintSolver()->getLastCollisionResult(); - const auto nContacts = static_cast(collisionResult.contacts.size()); + const auto nContacts = static_cast(collisionResult.getNumContacts()); const auto nSkeletons = getNumSkeletons(); Eigen::VectorXd state(getIndex(nSkeletons) + 6 * nContacts); @@ -525,8 +525,8 @@ void World::bake() for (auto i = 0; i < nContacts; ++i) { auto begin = getIndex(nSkeletons) + i * 6; - state.segment(begin, 3) = collisionResult.contacts[i].point; - state.segment(begin + 3, 3) = collisionResult.contacts[i].force; + state.segment(begin, 3) = collisionResult.getContact(i).point; + state.segment(begin + 3, 3) = collisionResult.getContact(i).force; } mRecording->addState(state); diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index e4fa281734226..d6b447078dc26 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -641,16 +641,15 @@ void testBoxBox(const std::shared_ptr& cd, Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); - const auto numContacts = result.contacts.size(); + const auto numContacts = result.getNumContacts(); const auto checkNumContacts = (numContacts <= 4u); EXPECT_TRUE(checkNumContacts); if (!checkNumContacts) std::cout << "# of contants: " << numContacts << "\n"; - for (auto i = 0u; i < numContacts; ++i) + for (const auto& contact : result.getContacts()) { - const auto& contact = result.contacts[i]; const auto& point = contact.point; const auto result = checkBoundingBox(min, max, point, tol); @@ -720,24 +719,24 @@ void testOptions(const std::shared_ptr& cd) collision::Option option; collision::Result result; - result.contacts.clear(); + result.clear(); option.maxNumContacts = 1000u; option.binaryCheck = false; EXPECT_TRUE(group->detect(option, result)); - EXPECT_EQ(result.contacts.size(), 4u); + EXPECT_EQ(result.getNumContacts(), 4u); - result.contacts.clear(); + result.clear(); option.maxNumContacts = 2u; option.binaryCheck = false; EXPECT_TRUE(group->detect(option, result)); - EXPECT_EQ(result.contacts.size(), 2u); + EXPECT_EQ(result.getNumContacts(), 2u); group->registerShapeFrame(simpleFrame3.get()); - result.contacts.clear(); + result.clear(); option.maxNumContacts = 1e+3; option.binaryCheck = true; EXPECT_TRUE(group->detect(option, result)); - EXPECT_EQ(result.contacts.size(), 1u); + EXPECT_EQ(result.getNumContacts(), 1u); } //============================================================================== diff --git a/unittests/testConstraint.cpp b/unittests/testConstraint.cpp index 501c6aaa32a84..098a5587f1561 100644 --- a/unittests/testConstraint.cpp +++ b/unittests/testConstraint.cpp @@ -189,11 +189,9 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) world->step(); const auto& result = world->getConstraintSolver()->getLastCollisionResult(); - const auto& contacts = result.contacts; - for (size_t j = 0; j < contacts.size(); ++j) + for (const auto& contact : result.getContacts()) { - const Contact& contact = contacts[j]; Vector3d pos1 = sphere->getTransform().inverse() * contact.point; Vector3d vel1 = sphere->getLinearVelocity(pos1); From c3223828f0920fb137c235a777c80ec94c6f647a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 16:18:54 -0400 Subject: [PATCH 44/67] Add ShpeFrame accessors to CollisionGroup --- dart/collision/CollisionDetector.h | 2 -- dart/collision/CollisionGroup.cpp | 13 +++++++++++++ dart/collision/CollisionGroup.h | 7 +++++++ 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index dbea9fa936a8b..e7cef42cef410 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -96,7 +96,6 @@ class CollisionDetector : public std::enable_shared_from_this /// Claim CollisionObject associated with shapeFrame. New CollisionObject /// will be created if it hasn't created yet for shapeFrame. CollisionObject* claimCollisionObject(const dynamics::ShapeFrame* shapeFrame); - // TODO(JS): Maybe WeakShapeFramePtr /// Reclaim a CollisionObject. The CollisionObject will be destroyed if no /// CollisionGroup holds it. @@ -127,7 +126,6 @@ class CollisionDetector::CollisionObjectManager /// will be created if it hasn't created yet for shapeFrame. virtual CollisionObject* claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) = 0; - // TODO(JS): Maybe WeakShapeFramePtr /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject /// will be destroyed if no CollisionGroup holds it. diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 3ceb015c508e5..085c2dd697853 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -267,6 +267,19 @@ size_t CollisionGroup::getNumShapeFrames() const return mShapeFrames.size(); } +//============================================================================== +const dynamics::ShapeFrame*CollisionGroup::getShapeFrame(size_t index) const +{ + common::getVectorObjectIfAvailable(index, mShapeFrames); +} + +//============================================================================== +const std::vector +CollisionGroup::getShapeFrames() const +{ + return mShapeFrames; +} + //============================================================================== void CollisionGroup::update() { diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index c8db421f5d0ce..5a71dda23b315 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -138,6 +138,13 @@ class CollisionGroup /// Return number of ShapeFrames added to this CollisionGroup size_t getNumShapeFrames() const; + /// Return index-th ShapeFrames registered to this CollisionGroup. Return + /// nullptr if the index is out of the range. + const dynamics::ShapeFrame* getShapeFrame(size_t index) const; + + /// Return all the ShapeFrames registered to this CollisionGroup + const std::vector getShapeFrames() const; + /// Update engine data. This function should be called before the collision /// detection is performed by the engine in most cases. void update(); From 123e87d1420debd42fa151de672edac62f8141a8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 19:32:35 -0400 Subject: [PATCH 45/67] Polish BulletCollisionDetector class --- .../bullet/BulletCollisionDetector.cpp | 395 +++++++++--------- .../bullet/BulletCollisionDetector.h | 27 +- .../collision/bullet/BulletCollisionGroup.cpp | 50 +-- dart/collision/bullet/BulletCollisionGroup.h | 13 +- dart/collision/dart/DARTCollisionGroup.cpp | 23 - dart/collision/dart/DARTCollisionGroup.h | 9 - dart/collision/fcl/FCLCollisionGroup.cpp | 25 -- dart/collision/fcl/FCLCollisionGroup.h | 13 +- 8 files changed, 212 insertions(+), 343 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 59deaff3d0ea1..b8621b38880d9 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -125,6 +125,16 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, void convertContacts( btCollisionWorld* collWorld, const Option& option, Result& result); +BulletCollisionDetector::BulletCollsionShapePack +createBulletEllipsoidMesh(float sizeX, float sizeY, float sizeZ); + +BulletCollisionDetector::BulletCollsionShapePack +createBulletCollisionShapePackFromAssimpScene( + const Eigen::Vector3d& scale, const aiScene* scene); + +BulletCollisionDetector::BulletCollsionShapePack +createBulletCollisionShapePackFromAssimpMesh(const aiMesh* mesh); + } // anonymous namespace @@ -264,11 +274,11 @@ void BulletCollisionDetector::notifyCollisionObjectDestorying( } //============================================================================== -BulletCollisionDetector::BulletCollsionPack +BulletCollisionDetector::BulletCollsionShapePack BulletCollisionDetector::claimBulletCollisionGeometry( const dynamics::ConstShapePtr& shape) { - BulletCollsionPack pack; + BulletCollsionShapePack pack; auto findResult = mShapeMap.find(shape); if (mShapeMap.end() != findResult) @@ -283,7 +293,7 @@ BulletCollisionDetector::claimBulletCollisionGeometry( } else { - pack = createBulletCollisionShape(shape); + pack = createBulletCollisionShapePack(shape); mShapeMap[shape] = std::make_pair(pack, 1u); } @@ -307,8 +317,180 @@ void BulletCollisionDetector::reclaimBulletCollisionGeometry( } //============================================================================== -BulletCollisionDetector::BulletCollsionPack -BulletCollisionDetector::createEllipsoidMesh( +BulletCollisionDetector::BulletCollsionShapePack +BulletCollisionDetector::createBulletCollisionShapePack( + const dynamics::ConstShapePtr& shape) +{ + using dynamics::Shape; + using dynamics::BoxShape; + using dynamics::EllipsoidShape; + using dynamics::CylinderShape; + using dynamics::PlaneShape; + using dynamics::MeshShape; + using dynamics::SoftMeshShape; + + BulletCollsionShapePack pack; + + auto& bulletCollisionShape = pack.collisionShape; + + switch (shape->getShapeType()) + { + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); + + const auto box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); + + bulletCollisionShape.reset(new btBoxShape(convertVector3(size*0.5))); + + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + + const auto ellipsoid = static_cast(shape.get()); + const Eigen::Vector3d& size = ellipsoid->getSize(); + + if (ellipsoid->isSphere()) + bulletCollisionShape.reset(new btSphereShape(size[0] * 0.5)); + else + pack = createBulletEllipsoidMesh(size[0], size[1], size[2]); + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + + const auto cylinder = static_cast(shape.get()); + const auto radius = cylinder->getRadius(); + const auto height = cylinder->getHeight(); + const auto size = btVector3(radius, radius, height * 0.5); + + bulletCollisionShape.reset(new btCylinderShapeZ(size)); + + break; + } + case Shape::PLANE: + { + assert(dynamic_cast(shape.get())); + + const auto plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + + bulletCollisionShape.reset(new btStaticPlaneShape( + convertVector3(normal), offset)); + + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + + const auto shapeMesh = static_cast(shape.get()); + const auto scale = shapeMesh->getScale(); + const auto mesh = shapeMesh->getMesh(); + + pack = createBulletCollisionShapePackFromAssimpScene(scale, mesh); + + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + + const auto softMeshShape = static_cast(shape.get()); + const auto mesh = softMeshShape->getAssimpMesh(); + + pack = createBulletCollisionShapePackFromAssimpMesh(mesh); + + break; + } + default: + { + dterr << "[BulletCollisionObjectData::init] " + << "Attempting to create unsupported shape type '" + << shape->getShapeType() << "'.\n"; + + bulletCollisionShape.reset(new btSphereShape(0.1)); + + break; + } + } + + return pack; +} + + + +namespace { + +//============================================================================== +Contact convertContact(const btManifoldPoint& bulletManifoldPoint, + const BulletCollisionObject::UserData* userData1, + const BulletCollisionObject::UserData* userData2) +{ + assert(userData1); + assert(userData2); + + Contact contact; + + contact.point = convertVector3(bulletManifoldPoint.getPositionWorldOnA()); + contact.normal = convertVector3(-bulletManifoldPoint.m_normalWorldOnB); + contact.penetrationDepth = -bulletManifoldPoint.m_distance1; + contact.collisionObject1 = userData1->collisionObject; + contact.collisionObject2 = userData2->collisionObject; + + return contact; +} + +//============================================================================== +void convertContacts( + btCollisionWorld* collWorld, const Option& option, Result& result) +{ + assert(collWorld); + + auto dispatcher = collWorld->getDispatcher(); + assert(dispatcher); + + auto numManifolds = dispatcher->getNumManifolds(); + + for (auto i = 0; i < numManifolds; ++i) + { + auto contactManifold = dispatcher->getManifoldByIndexInternal(i); + const auto bulletCollObj0 = contactManifold->getBody0(); + const auto bulletCollObj1 = contactManifold->getBody1(); + + auto userPointer0 = bulletCollObj0->getUserPointer(); + auto userPointer1 = bulletCollObj1->getUserPointer(); + + auto userDataA + = static_cast(userPointer1); + auto userDataB + = static_cast(userPointer0); + + auto numContacts = contactManifold->getNumContacts(); + + for (auto j = 0; j < numContacts; ++j) + { + auto& cp = contactManifold->getContactPoint(j); + + result.addContact(convertContact(cp, userDataA, userDataB)); + + if (option.binaryCheck) + return; + + if (result.getNumContacts() >= option.maxNumContacts) + break; + } + } +} + +//============================================================================== +BulletCollisionDetector::BulletCollsionShapePack createBulletEllipsoidMesh( float sizeX, float sizeY, float sizeZ) { float v[59][3] = @@ -490,7 +672,7 @@ BulletCollisionDetector::createEllipsoidMesh( {56, 49, 58} }; - BulletCollsionPack pack; + BulletCollisionDetector::BulletCollsionShapePack pack; pack.triMesh.reset(new btTriangleMesh()); for (auto i = 0u; i < 112; ++i) @@ -521,22 +703,22 @@ BulletCollisionDetector::createEllipsoidMesh( } //============================================================================== -BulletCollisionDetector::BulletCollsionPack -BulletCollisionDetector::createMesh( - const Eigen::Vector3d& scale, const aiScene* mesh) +BulletCollisionDetector::BulletCollsionShapePack +createBulletCollisionShapePackFromAssimpScene( + const Eigen::Vector3d& scale, const aiScene* scene) { - BulletCollsionPack pack; + BulletCollisionDetector::BulletCollsionShapePack pack; pack.triMesh.reset(new btTriangleMesh()); - for (unsigned int i = 0; i < mesh->mNumMeshes; i++) + for (auto i = 0u; i < scene->mNumMeshes; ++i) { - for (unsigned int j = 0; j < mesh->mMeshes[i]->mNumFaces; j++) + for (auto j = 0u; j < scene->mMeshes[i]->mNumFaces; ++j) { btVector3 vertices[3]; - for (unsigned int k = 0; k < 3; k++) + for (auto k = 0u; k < 3; ++k) { - const aiVector3D& vertex = mesh->mMeshes[i]->mVertices[ - mesh->mMeshes[i]->mFaces[j].mIndices[k]]; + const aiVector3D& vertex = scene->mMeshes[i]->mVertices[ + scene->mMeshes[i]->mFaces[j].mIndices[k]]; vertices[k] = btVector3(vertex.x * scale[0], vertex.y * scale[1], vertex.z * scale[2]); @@ -554,10 +736,10 @@ BulletCollisionDetector::createMesh( } //============================================================================== -BulletCollisionDetector::BulletCollsionPack -BulletCollisionDetector::createSoftMesh(const aiMesh* mesh) +BulletCollisionDetector::BulletCollsionShapePack +createBulletCollisionShapePackFromAssimpMesh(const aiMesh* mesh) { - BulletCollsionPack pack; + BulletCollisionDetector::BulletCollsionShapePack pack; pack.triMesh.reset(new btTriangleMesh()); for (auto i = 0u; i < mesh->mNumFaces; ++i) @@ -579,183 +761,6 @@ BulletCollisionDetector::createSoftMesh(const aiMesh* mesh) return pack; } -//============================================================================== -BulletCollisionDetector::BulletCollsionPack -BulletCollisionDetector::createBulletCollisionShape( - const dynamics::ConstShapePtr& shape) -{ - using dynamics::Shape; - using dynamics::BoxShape; - using dynamics::EllipsoidShape; - using dynamics::CylinderShape; - using dynamics::PlaneShape; - using dynamics::MeshShape; - using dynamics::SoftMeshShape; - - BulletCollsionPack pack; - - auto& bulletCollisionShape = pack.collisionShape; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - - const auto box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); - - bulletCollisionShape.reset(new btBoxShape(convertVector3(size*0.5))); - - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - - const auto ellipsoid = static_cast(shape.get()); - const Eigen::Vector3d& size = ellipsoid->getSize(); - - if (ellipsoid->isSphere()) - { - bulletCollisionShape.reset(new btSphereShape(size[0] * 0.5)); - } - else - { - // TODO(JS): Add mesh for ellipsoid - } - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - - const auto cylinder = static_cast(shape.get()); - const auto radius = cylinder->getRadius(); - const auto height = cylinder->getHeight(); - const auto size = btVector3(radius, radius, height * 0.5); - - bulletCollisionShape.reset(new btCylinderShapeZ(size)); - - break; - } - case Shape::PLANE: - { - assert(dynamic_cast(shape.get())); - - const auto plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - - bulletCollisionShape.reset(new btStaticPlaneShape( - convertVector3(normal), offset)); - - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - - const auto shapeMesh = static_cast(shape.get()); - const auto scale = shapeMesh->getScale(); - const auto mesh = shapeMesh->getMesh(); - - pack = createMesh(scale, mesh); - - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - - const auto softMeshShape = static_cast(shape.get()); - const auto mesh = softMeshShape->getAssimpMesh(); - - pack = createSoftMesh(mesh); - - break; - } - default: - { - dterr << "[BulletCollisionObjectData::init] " - << "Attempting to create unsupported shape type '" - << shape->getShapeType() << "'.\n"; - - bulletCollisionShape.reset(new btSphereShape(0.1)); - - break; - } - } - - return pack; -} - - - -namespace { - -//============================================================================== -Contact convertContact(const btManifoldPoint& bulletManifoldPoint, - const BulletCollisionObject::UserData* userData1, - const BulletCollisionObject::UserData* userData2) -{ - assert(userData1); - assert(userData2); - - Contact contact; - - contact.point = convertVector3(bulletManifoldPoint.getPositionWorldOnA()); - contact.normal = convertVector3(-bulletManifoldPoint.m_normalWorldOnB); - contact.penetrationDepth = -bulletManifoldPoint.m_distance1; - contact.collisionObject1 = userData1->collisionObject; - contact.collisionObject2 = userData2->collisionObject; - - return contact; -} - -//============================================================================== -void convertContacts( - btCollisionWorld* collWorld, const Option& option, Result& result) -{ - assert(collWorld); - - auto dispatcher = collWorld->getDispatcher(); - assert(dispatcher); - - auto numManifolds = dispatcher->getNumManifolds(); - - for (auto i = 0; i < numManifolds; ++i) - { - auto contactManifold = dispatcher->getManifoldByIndexInternal(i); - const auto bulletCollObj0 = contactManifold->getBody0(); - const auto bulletCollObj1 = contactManifold->getBody1(); - - auto userPointer0 = bulletCollObj0->getUserPointer(); - auto userPointer1 = bulletCollObj1->getUserPointer(); - - auto userDataA - = static_cast(userPointer1); - auto userDataB - = static_cast(userPointer0); - - auto numContacts = contactManifold->getNumContacts(); - - for (auto j = 0; j < numContacts; ++j) - { - auto& cp = contactManifold->getContactPoint(j); - - result.addContact(convertContact(cp, userDataA, userDataB)); - - if (option.binaryCheck) - return; - - if (result.getNumContacts() >= option.maxNumContacts) - break; - } - } -} - } // anonymous namespace } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 367300371d4c6..342018f887793 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -53,6 +53,12 @@ class BulletCollisionDetector : public CollisionDetector friend class CollisionDetector; + struct BulletCollsionShapePack + { + std::shared_ptr collisionShape; + std::shared_ptr triMesh; + }; + static std::shared_ptr create(); /// Constructor @@ -77,13 +83,6 @@ class BulletCollisionDetector : public CollisionDetector protected: - struct BulletCollsionPack - { - std::shared_ptr collisionShape; - std::shared_ptr triMesh; - // TODO(JS): change to unique_ptr - }; - /// Constructor BulletCollisionDetector(); @@ -94,26 +93,18 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited void notifyCollisionObjectDestorying(CollisionObject* collObj) override; - /// - BulletCollsionPack claimBulletCollisionGeometry( + BulletCollsionShapePack claimBulletCollisionGeometry( const dynamics::ConstShapePtr& shape); // Documentation inherited void reclaimBulletCollisionGeometry(const dynamics::ConstShapePtr& shape); - BulletCollsionPack createEllipsoidMesh(float sizeX, float sizeY, float sizeZ); - - BulletCollsionPack createMesh( - const Eigen::Vector3d& scale, const aiScene* mesh); - - BulletCollsionPack createSoftMesh(const aiMesh* mesh); - - BulletCollsionPack createBulletCollisionShape( + BulletCollsionShapePack createBulletCollisionShapePack( const dynamics::ConstShapePtr& shape); protected: - using ShapeMapValue = std::pair; + using ShapeMapValue = std::pair; std::map mShapeMap; diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 4c9c75674f1f5..e153a9ba27af2 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -58,47 +58,6 @@ BulletCollisionGroup::BulletCollisionGroup( assert(mCollisionDetector); } -//============================================================================== -BulletCollisionGroup::BulletCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame) - : CollisionGroup(collisionDetector), - mBulletProadphaseAlg(new btDbvtBroadphase()), - mBulletCollisionConfiguration(new btDefaultCollisionConfiguration()), - mBulletDispatcher( - new btCollisionDispatcher(mBulletCollisionConfiguration.get())), - mBulletCollisionWorld( - new btCollisionWorld(mBulletDispatcher.get(), - mBulletProadphaseAlg.get(), - mBulletCollisionConfiguration.get())) -{ - assert(mCollisionDetector); - assert(shapeFrame); - - registerShapeFrame(shapeFrame); -} - -//============================================================================== -BulletCollisionGroup::BulletCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames) - : CollisionGroup(collisionDetector), - mBulletProadphaseAlg( - new btDbvtBroadphase()), - mBulletCollisionConfiguration( - new btDefaultCollisionConfiguration()), - mBulletDispatcher( - new btCollisionDispatcher(mBulletCollisionConfiguration.get())), - mBulletCollisionWorld( - new btCollisionWorld(mBulletDispatcher.get(), - mBulletProadphaseAlg.get(), - mBulletCollisionConfiguration.get())) -{ - assert(mCollisionDetector); - - registerShapeFrames(shapeFrames); -} - //============================================================================== BulletCollisionGroup::~BulletCollisionGroup() { @@ -108,9 +67,7 @@ BulletCollisionGroup::~BulletCollisionGroup() //============================================================================== void BulletCollisionGroup::initializeEngineData() { - btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); - // dispatchInfo.m_timeStep = 0.001; - dispatchInfo.m_stepCount = 0; + // Do nothing } //============================================================================== @@ -162,11 +119,6 @@ void BulletCollisionGroup::notifyAllCollisionObjectsRemoved() //============================================================================== void BulletCollisionGroup::updateEngineData() { - // Setting up broadphase collision detection options - btDispatcherInfo& dispatchInfo = mBulletCollisionWorld->getDispatchInfo(); - // dispatchInfo.m_timeStep = 0.001; - dispatchInfo.m_stepCount = 0; - mBulletCollisionWorld->updateAabbs(); } diff --git a/dart/collision/bullet/BulletCollisionGroup.h b/dart/collision/bullet/BulletCollisionGroup.h index 981c2ff26f151..8342c2cbe32bc 100644 --- a/dart/collision/bullet/BulletCollisionGroup.h +++ b/dart/collision/bullet/BulletCollisionGroup.h @@ -51,18 +51,7 @@ class BulletCollisionGroup : public CollisionGroup friend class BulletCollisionDetector; /// Constructor - BulletCollisionGroup( - const CollisionDetectorPtr& collisionDetector); - - /// Constructor - BulletCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame); - - /// Constructor - BulletCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames); + BulletCollisionGroup(const CollisionDetectorPtr& collisionDetector); /// Destructor virtual ~BulletCollisionGroup(); diff --git a/dart/collision/dart/DARTCollisionGroup.cpp b/dart/collision/dart/DARTCollisionGroup.cpp index 22565f80afb42..299ee1d930a31 100644 --- a/dart/collision/dart/DARTCollisionGroup.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -49,29 +49,6 @@ DARTCollisionGroup::DARTCollisionGroup( assert(mCollisionDetector); } -//============================================================================== -DARTCollisionGroup::DARTCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame) - : CollisionGroup(collisionDetector) -{ - assert(mCollisionDetector); - assert(shapeFrame); - - registerShapeFrame(shapeFrame); -} - -//============================================================================== -DARTCollisionGroup::DARTCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames) - : CollisionGroup(collisionDetector) -{ - assert(mCollisionDetector); - - registerShapeFrames(shapeFrames); -} - //============================================================================== DARTCollisionGroup::~DARTCollisionGroup() { diff --git a/dart/collision/dart/DARTCollisionGroup.h b/dart/collision/dart/DARTCollisionGroup.h index 27018570345cc..2761e539d2d48 100644 --- a/dart/collision/dart/DARTCollisionGroup.h +++ b/dart/collision/dart/DARTCollisionGroup.h @@ -51,15 +51,6 @@ class DARTCollisionGroup : public CollisionGroup /// Constructor DARTCollisionGroup(const CollisionDetectorPtr& collisionDetector); - /// Constructor - DARTCollisionGroup(const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame); - - /// Constructor - DARTCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames); - /// Destructor virtual ~DARTCollisionGroup(); diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index 96defffde2544..d66ae91627e1d 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -51,31 +51,6 @@ FCLCollisionGroup::FCLCollisionGroup( assert(mCollisionDetector); } -//============================================================================== -FCLCollisionGroup::FCLCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame) - : CollisionGroup(collisionDetector), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) -{ - assert(mCollisionDetector); - assert(shapeFrame); - - registerShapeFrame(shapeFrame); -} - -//============================================================================== -FCLCollisionGroup::FCLCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames) - : CollisionGroup(collisionDetector), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) -{ - assert(mCollisionDetector); - - registerShapeFrames(shapeFrames); -} - //============================================================================== FCLCollisionGroup::~FCLCollisionGroup() { diff --git a/dart/collision/fcl/FCLCollisionGroup.h b/dart/collision/fcl/FCLCollisionGroup.h index 3a5b848f37daf..6cfd7d465d9fc 100644 --- a/dart/collision/fcl/FCLCollisionGroup.h +++ b/dart/collision/fcl/FCLCollisionGroup.h @@ -56,18 +56,7 @@ class FCLCollisionGroup : public CollisionGroup using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; /// Constructor - FCLCollisionGroup( - const CollisionDetectorPtr& collisionDetector); - - /// Constructor - FCLCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const dynamics::ShapeFrame* shapeFrame); - - /// Constructor - FCLCollisionGroup( - const CollisionDetectorPtr& collisionDetector, - const std::vector& shapeFrames); + FCLCollisionGroup(const CollisionDetectorPtr& collisionDetector); /// Destructor virtual ~FCLCollisionGroup(); From e6739511f3acec6c957a94e2698f94d8a8c366e5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 19:41:30 -0400 Subject: [PATCH 46/67] Add more collision test --- unittests/testCollision.cpp | 93 ++++++++++--------------------------- 1 file changed, 25 insertions(+), 68 deletions(-) diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index d6b447078dc26..b40d9dce86990 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -772,111 +772,68 @@ TEST_F(COLLISION, Options) } //============================================================================== -void testBodyNodes(const std::shared_ptr& cd) +void testCreateCollisionGroups(const std::shared_ptr& cd) { Eigen::Vector3d size(1.0, 1.0, 1.0); Eigen::Vector3d pos1(0.0, 0.0, 0.0); Eigen::Vector3d pos2(0.5, 0.0, 0.0); - auto boxSkel1 = createBox(size, pos1); - auto boxSkel2 = createBox(size, pos2); + auto boxSkeleton1 = createBox(size, pos1); + auto boxSkeleton2 = createBox(size, pos2); - auto boxBody1 = boxSkel1->getBodyNode(0u); - auto boxBody2 = boxSkel2->getBodyNode(0u); + auto boxBodyNode1 = boxSkeleton1->getBodyNode(0u); + auto boxBodyNode2 = boxSkeleton2->getBodyNode(0u); - auto boxShapeNode1 = boxBody1->getShapeNodesWith()[0]; - auto boxShapeNode2 = boxBody2->getShapeNodesWith()[0]; + auto boxShapeNode1 = boxBodyNode1->getShapeNodesWith()[0]; + auto boxShapeNode2 = boxBodyNode2->getShapeNodesWith()[0]; collision::Option option; collision::Result result; - auto group1 = cd->createCollisionGroup(boxShapeNode1); - auto group2 = cd->createCollisionGroup(boxShapeNode2); + auto skeletonGroup1 = cd->createCollisionGroup(boxSkeleton1.get()); + auto skeletonGroup2 = cd->createCollisionGroup(boxSkeleton2.get()); - EXPECT_TRUE(group1->detect(group2.get(), option, result)); - EXPECT_TRUE(cd->detect(group1.get(), group2.get(), option, result)); + auto bodyNodeGroup1 = cd->createCollisionGroup(boxBodyNode1); + auto bodyNodeGroup2 = cd->createCollisionGroup(boxBodyNode2); + + auto shapeNodeGroup1 = cd->createCollisionGroup(boxShapeNode1); + auto shapeNodeGroup2 = cd->createCollisionGroup(boxShapeNode2); + + EXPECT_TRUE(skeletonGroup1->detect(skeletonGroup2.get(), option, result)); + EXPECT_TRUE(bodyNodeGroup1->detect(bodyNodeGroup2.get(), option, result)); + EXPECT_TRUE(shapeNodeGroup1->detect(shapeNodeGroup2.get(), option, result)); } //============================================================================== -TEST_F(COLLISION, BodyNodeNodes) +TEST_F(COLLISION, CreateCollisionGroupFromVariousObject) { auto fcl_mesh_dart = FCLCollisionDetector::create(); fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); - testBodyNodes(fcl_mesh_dart); + testCreateCollisionGroups(fcl_mesh_dart); // auto fcl_prim_fcl = FCLCollisionDetector::create(); // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); - // testBodyNodes(fcl_prim_fcl); + // testCreateCollisionGroups(fcl_prim_fcl); // auto fcl_mesh_fcl = FCLCollisionDetector::create(); // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); - // testBodyNodes(fcl_mesh_fcl); + // testCreateCollisionGroups(fcl_mesh_fcl); // auto fcl_mesh_fcl = FCLCollisionDetector::create(); // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); - // testBodyNodes(fcl_mesh_fcl); + // testCreateCollisionGroups(fcl_mesh_fcl); #if HAVE_BULLET_COLLISION auto bullet = BulletCollisionDetector::create(); - testBodyNodes(bullet); + testCreateCollisionGroups(bullet); #endif auto dart = DARTCollisionDetector::create(); - testBodyNodes(dart); -} - -//============================================================================== -void testSkeletons(const std::shared_ptr& /*cd*/) -{ -// Eigen::Vector3d size(1.0, 1.0, 1.0); -// Eigen::Vector3d pos1(0.0, 0.0, 0.0); -// Eigen::Vector3d pos2(0.5, 0.0, 0.0); - -// auto boxSkel1 = createBox(size, pos1); -// auto boxSkel2 = createBox(size, pos2); - -// collision::Option option; -// collision::Result result; - -// auto hit = cd->detect(boxSkel1, boxSkel2, option, result); - -// EXPECT_TRUE(hit); -} - -//============================================================================== -TEST_F(COLLISION, Skeletons) -{ - auto fcl_mesh_dart = FCLCollisionDetector::create(); - fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); - fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); - testSkeletons(fcl_mesh_dart); - - // auto fcl_prim_fcl = FCLCollisionDetector::create(); - // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); - // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); - // testSkeletons(fcl_prim_fcl); - - // auto fcl_mesh_fcl = FCLCollisionDetector::create(); - // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); - // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); - // testSkeletons(fcl_mesh_fcl); - - // auto fcl_mesh_fcl = FCLCollisionDetector::create(); - // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); - // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); - // testSkeletons(fcl_mesh_fcl); - -//#if HAVE_BULLET_COLLISION -// auto bullet = BulletCollisionDetector::create(); -// testSkeletons(bullet); -//#endif - -// auto dart = DARTCollisionDetector::create(); -// testSkeletons(dart); + testCreateCollisionGroups(dart); } //============================================================================== From 40d9a382bc1467132984183ab885d29f6e4eddbe Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 28 Mar 2016 20:30:52 -0400 Subject: [PATCH 47/67] Fix a mistake in CollisionGroup::getShapeFrame() --- dart/collision/CollisionGroup.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 085c2dd697853..730a187098809 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -268,9 +268,9 @@ size_t CollisionGroup::getNumShapeFrames() const } //============================================================================== -const dynamics::ShapeFrame*CollisionGroup::getShapeFrame(size_t index) const +const dynamics::ShapeFrame* CollisionGroup::getShapeFrame(size_t index) const { - common::getVectorObjectIfAvailable(index, mShapeFrames); + return common::getVectorObjectIfAvailable(index, mShapeFrames); } //============================================================================== From 97c97362c1606cc507a2e0136b3272ce7ce37f33 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 30 Mar 2016 20:07:25 -0400 Subject: [PATCH 48/67] Update CollisionGroup in response to @mkoval 's comments - rename [register/unregister]ShapeFrames[Of] to [add/remove]ShapeFrames[Of] - update comment on [add/remove]ShapeFrames[Of] - unnecessary EIGEN_MAKE_ALIGNED_OPERATOR_NEW in struct Contact - update accessibility update() and member variables --- dart/collision/CollisionDetector.h | 2 +- dart/collision/CollisionGroup.cpp | 147 +++------------ dart/collision/CollisionGroup.h | 173 +++++++++++------- dart/collision/Contact.h | 3 - .../bullet/BulletCollisionDetector.cpp | 6 +- .../collision/bullet/BulletCollisionGroup.cpp | 4 +- dart/collision/bullet/BulletCollisionGroup.h | 2 +- dart/collision/dart/DARTCollisionGroup.cpp | 4 +- dart/collision/dart/DARTCollisionGroup.h | 2 +- dart/collision/detail/CollisionDetector.h | 2 +- dart/collision/detail/CollisionGroup.h | 145 ++++++++++++++- dart/collision/fcl/FCLCollisionDetector.cpp | 6 +- dart/collision/fcl/FCLCollisionGroup.cpp | 4 +- dart/collision/fcl/FCLCollisionGroup.h | 2 +- dart/constraint/ConstraintSolver.cpp | 6 +- unittests/testCollision.cpp | 2 +- 16 files changed, 284 insertions(+), 226 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index e7cef42cef410..8a95569210eb1 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -68,7 +68,7 @@ class CollisionDetector : public std::enable_shared_from_this virtual std::shared_ptr createCollisionGroup() = 0; /// Create a collision group from any objects that are supported by - /// CollisionGroup::registerShapeFramesOf(). Currently, the supporting objects + /// CollisionGroup::addShapeFramesOf(). Currently, the supporting objects /// are ShapeFrame, std::vector, CollisionGroup, BodyNode, and /// Skeleton. template diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 730a187098809..4f08073d738a5 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -61,13 +61,19 @@ CollisionGroup::~CollisionGroup() } //============================================================================== -CollisionDetector* CollisionGroup::getCollisionDetector() const +CollisionDetector* CollisionGroup::getCollisionDetector() { return mCollisionDetector.get(); } //============================================================================== -void CollisionGroup::registerShapeFrame(const dynamics::ShapeFrame* shapeFrame) +const CollisionDetector* CollisionGroup::getCollisionDetector() const +{ + return mCollisionDetector.get(); +} + +//============================================================================== +void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) { if (!shapeFrame) return; @@ -84,69 +90,21 @@ void CollisionGroup::registerShapeFrame(const dynamics::ShapeFrame* shapeFrame) } //============================================================================== -void CollisionGroup::registerShapeFrames( +void CollisionGroup::addShapeFrames( const std::vector& shapeFrames) { for (const auto& shapeFrame : shapeFrames) - registerShapeFrame(shapeFrame); + addShapeFrame(shapeFrame); } //============================================================================== -void CollisionGroup::registerShapeFrames() +void CollisionGroup::addShapeFramesOf() { // Do nothing } //============================================================================== -void CollisionGroup::registerShapeFramesOf( - const dynamics::ShapeFrame* shapeFrame) -{ - registerShapeFrame(shapeFrame); -} - -//============================================================================== -void CollisionGroup::registerShapeFramesOf( - const std::vector& shapeFrames) -{ - registerShapeFrames(shapeFrames); -} - -//============================================================================== -void CollisionGroup::registerShapeFramesOf(const CollisionGroup* other) -{ - assert(other); - - if (other && this != other) - { - for (const auto& shapeFrame : other->mShapeFrames) - registerShapeFrame(shapeFrame); - } -} - -//============================================================================== -void CollisionGroup::registerShapeFramesOf(const dynamics::BodyNode* bodyNode) -{ - assert(bodyNode); - - auto collisionShapeNodes - = bodyNode->getShapeNodesWith(); - - for (auto& shapeNode : collisionShapeNodes) - registerShapeFrame(shapeNode); -} - -//============================================================================== -void CollisionGroup::registerShapeFramesOf(const dynamics::Skeleton* skel) -{ - assert(skel); - - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - registerShapeFramesOf(skel->getBodyNode(i)); -} - -//============================================================================== -void CollisionGroup::unregisterShapeFrame( +void CollisionGroup::removeShapeFrame( const dynamics::ShapeFrame* shapeFrame) { if (!shapeFrame) @@ -171,74 +129,19 @@ void CollisionGroup::unregisterShapeFrame( } //============================================================================== -void CollisionGroup::unregisterShapeFrames( +void CollisionGroup::removeShapeFrames( const std::vector& shapeFrames) { for (const auto& shapeFrame : shapeFrames) - unregisterShapeFrame(shapeFrame); + removeShapeFrame(shapeFrame); } //============================================================================== -void CollisionGroup::unregisterShapeFramesOf() +void CollisionGroup::removeShapeFramesOf() { // Do nothing } -//============================================================================== -void CollisionGroup::unregisterShapeFramesOf( - const dynamics::ShapeFrame* shapeFrame) -{ - unregisterShapeFrame(shapeFrame); -} - -//============================================================================== -void CollisionGroup::unregisterShapeFramesOf( - const std::vector& shapeFrames) -{ - unregisterShapeFrames(shapeFrames); -} - -//============================================================================== -void CollisionGroup::unregisterShapeFramesOf(const CollisionGroup* other) -{ - assert(other); - - if (other) - { - if (this == other) - { - unregisterAllShapeFrames(); - return; - } - - for (const auto& shapeFrame : other->mShapeFrames) - unregisterShapeFrame(shapeFrame); - } -} - -//============================================================================== -void CollisionGroup::unregisterShapeFramesOf( - const dynamics::BodyNode* bodyNode) -{ - assert(bodyNode); - - auto collisionShapeNodes - = bodyNode->getShapeNodesWith(); - - for (auto& shapeNode : collisionShapeNodes) - unregisterShapeFrame(shapeNode); -} - -//============================================================================== -void CollisionGroup::unregisterShapeFramesOf(const dynamics::Skeleton* skel) -{ - assert(skel); - - auto numBodyNodes = skel->getNumBodyNodes(); - for (auto i = 0u; i < numBodyNodes; ++i) - unregisterShapeFramesOf(skel->getBodyNode(i)); -} - //============================================================================== void CollisionGroup::unregisterAllShapeFrames() { @@ -274,21 +177,12 @@ const dynamics::ShapeFrame* CollisionGroup::getShapeFrame(size_t index) const } //============================================================================== -const std::vector +const std::vector& CollisionGroup::getShapeFrames() const { return mShapeFrames; } -//============================================================================== -void CollisionGroup::update() -{ - for (auto& object : mCollisionObjects) - object->updateEngineData(); - - updateEngineData(); -} - //============================================================================== bool CollisionGroup::detect(const Option& option, Result& result) { @@ -308,5 +202,14 @@ const std::vector& CollisionGroup::getCollisionObjects() return mCollisionObjects; } +//============================================================================== +void CollisionGroup::updateEngineData() +{ + for (auto& object : mCollisionObjects) + object->updateEngineData(); + + updateCollisionGroupEngineData(); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 5a71dda23b315..7b87d0cf20c40 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -50,6 +50,9 @@ class CollisionGroup { public: + friend class FCLCollisionDetector; + friend class BulletCollisionDetector; + /// Constructor CollisionGroup(const CollisionDetectorPtr& collisionDetector); @@ -57,77 +60,107 @@ class CollisionGroup virtual ~CollisionGroup(); /// Return collision detection engine associated with this CollisionGroup - CollisionDetector* getCollisionDetector() const; - - /// Register a ShapeFrame to this CollisionGroup - void registerShapeFrame(const dynamics::ShapeFrame* shapeFrame); + CollisionDetector* getCollisionDetector(); - /// Register ShapeFrames to this CollisionGroup - void registerShapeFrames( - const std::vector& shapeFrames); - - /// Register all the ShapeFrames in the other CollisionGroups to this + /// Return (const) collision detection engine associated with this /// CollisionGroup - template - void registerShapeFrames(const First* first, const Others*... others); - - /// Do nothing. This function is for terminating the variadic template - /// function template registerShapeFrames(). - void registerShapeFrames(); + const CollisionDetector* getCollisionDetector() const; - /// As as registerShapeFrame() - void registerShapeFramesOf(const dynamics::ShapeFrame* shapeFrame); + /// Add a ShapeFrame to this CollisionGroup + void addShapeFrame(const dynamics::ShapeFrame* shapeFrame); - /// As as registerShapeFrames() - void registerShapeFramesOf( + /// Add ShapeFrames to this CollisionGroup + void addShapeFrames( const std::vector& shapeFrames); - /// Register all the ShapeFrames in the other CollisionGroup to this - /// CollisionGroup - void registerShapeFramesOf(const CollisionGroup* other); - - /// Register all the BodyNode's ShapeFrames having CollisionAddon to this - /// CollisionGroup - void registerShapeFramesOf(const dynamics::BodyNode* bodyNode); - - /// Register all the Skeleton's ShapeFrames having CollisionAddon to this - /// CollisionGroup - void registerShapeFramesOf(const dynamics::Skeleton* skeleton); - - /// Unregister a ShapeFrame from this CollisionGroup - void unregisterShapeFrame(const dynamics::ShapeFrame* shapeFrame); - - /// Unregister ShapeFrames from this CollisionGroup - void unregisterShapeFrames( - const std::vector& shapeFrames); - - /// Unregister all the ShapeFrames in the other CollisionGroups from this - /// CollisionGroup - template - void unregisterShapeFrames(const First* first, const Others*... others); - - /// Do nothing. This function is for terminating the variadic template - /// function template unregisterShapeFrames(). - void unregisterShapeFramesOf(); - - /// As as unregisterShapeFrame() - void unregisterShapeFramesOf(const dynamics::ShapeFrame* shapeFrame); - - /// As as unregisterShapeFrames() - void unregisterShapeFramesOf( + /// Add a ShapeFrame, and also add ShapeFrames of other various objects. + /// + /// The other various objects can be any of ShapeFrame, + /// std::vector, CollisionGroup, BodyNode, Skeleton, and + /// CollisionGroup. + /// + /// Note that this function adds only the ShapeFrames of each object at the + /// moment of this function is called. The aftwerward changes of the objects + /// does not affect on this CollisionGroup. + template + void addShapeFramesOf(const dynamics::ShapeFrame* shapeFrame, + const Others*... others); + + /// Add ShapeFrames, and also add ShapeFrames of other various objects. + template + void addShapeFramesOf( + const std::vector& shapeFrames, + const Others*... others); + + /// Add ShapeFrames of other CollisionGroup, and also add another ShapeFrames + /// of other various objects. + template + void addShapeFramesOf(const CollisionGroup* otherGroup, + const Others*... others); + + /// Add ShapeFrames of BodyNode, and also add another ShapeFrames of other + /// various objects. + template + void addShapeFramesOf(const dynamics::BodyNode* bodyNode, + const Others*... others); + + /// Add ShapeFrames of Skeleton, and also add another ShapeFrames of other + /// various objects. + template + void addShapeFramesOf(const dynamics::Skeleton* skeleton, + const Others*... others); + + /// Do nothing. This function is for terminating the recursive variadic + /// template function template addShapeFramesOf(). + void addShapeFramesOf(); + + /// Remove a ShapeFrame from this CollisionGroup + void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame); + + /// Remove ShapeFrames from this CollisionGroup + void removeShapeFrames( const std::vector& shapeFrames); - /// Unregister all the ShapeFrames in the other CollisionGroup from this - /// CollisionGroup - void unregisterShapeFramesOf(const CollisionGroup* other); - - /// Unregister all the BodyNode's ShapeFrames having CollisionAddon from this - /// CollisionGroup - void unregisterShapeFramesOf(const dynamics::BodyNode* bodyNode); - - /// Unregister all the Skeleton's ShapeFrames having CollisionAddon from this - /// CollisionGroup - void unregisterShapeFramesOf(const dynamics::Skeleton* skeleton); + /// Remove a ShapeFrame, and also remove ShapeFrames of other various objects. + /// + /// The other various objects can be any of ShapeFrame, + /// std::vector, CollisionGroup, BodyNode, Skeleton, and + /// CollisionGroup. + /// + /// Note that this function removes only the ShapeFrames of each object at the + /// moment of this function is called. The aftwerward changes of the objects + /// does not affect on this CollisionGroup. + template + void removeShapeFramesOf(const dynamics::ShapeFrame* shapeFrame, + const Others*... others); + + /// Remove ShapeFrames, and also remove ShapeFrames of other various objects. + template + void removeShapeFramesOf( + const std::vector& shapeFrames, + const Others*... others); + + /// Remove ShapeFrames of other CollisionGroup, and also remove another + /// ShapeFrames of other various objects. + template + void removeShapeFramesOf(const CollisionGroup* otherGroup, + const Others*... others); + + /// Remove ShapeFrames of BodyNode, and also remove another ShapeFrames of + /// other various objects. + template + void removeShapeFramesOf(const dynamics::BodyNode* bodyNode, + const Others*... others); + + /// Remove ShapeFrames of Skeleton, and also remove another ShapeFrames of + /// other various objects. + template + void removeShapeFramesOf(const dynamics::Skeleton* skeleton, + const Others*... others); + + /// Do nothing. This function is for terminating the recursive variadic + /// template function template removeShapeFramesOf(). + void removeShapeFramesOf(); /// Unregister all the ShapeFrames in this CollisionGroup void unregisterAllShapeFrames(); @@ -143,11 +176,7 @@ class CollisionGroup const dynamics::ShapeFrame* getShapeFrame(size_t index) const; /// Return all the ShapeFrames registered to this CollisionGroup - const std::vector getShapeFrames() const; - - /// Update engine data. This function should be called before the collision - /// detection is performed by the engine in most cases. - void update(); + const std::vector& getShapeFrames() const; /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); @@ -163,6 +192,10 @@ class CollisionGroup protected: + /// Update engine data. This function should be called before the collision + /// detection is performed by the engine in most cases. + void updateEngineData(); + /// Initialize the collision detection engine data such as broadphase /// algorithm. This function will be called whenever ShapeFrame is either /// added to or removed from this CollisionGroup. @@ -187,9 +220,9 @@ class CollisionGroup /// Update the collision detection engine data such as broadphase algorithm. /// This function will be called ahead of every collision checking. - virtual void updateEngineData() = 0; + virtual void updateCollisionGroupEngineData() = 0; -protected: +private: /// Collision detector CollisionDetectorPtr mCollisionDetector; diff --git a/dart/collision/Contact.h b/dart/collision/Contact.h index 53780f22776fa..7d3f5a51f5c0b 100644 --- a/dart/collision/Contact.h +++ b/dart/collision/Contact.h @@ -47,9 +47,6 @@ namespace collision { /// Contact information struct Contact { - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /// Contact point w.r.t. the world frame Eigen::Vector3d point; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index b8621b38880d9..23e09bc5eb0e3 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -186,7 +186,7 @@ bool BulletCollisionDetector::detect( return false; } - group->update(); + group->updateEngineData(); auto castedData = static_cast(group); auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); @@ -224,8 +224,8 @@ bool BulletCollisionDetector::detect( } auto group = common::make_unique(shared_from_this()); - group->registerShapeFrames(group1, group2); - group->update(); + group->addShapeFramesOf(group1, group2); + group->updateEngineData(); auto bulletCollisionWorld = group->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index e153a9ba27af2..0ad23c845f9b5 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -55,7 +55,7 @@ BulletCollisionGroup::BulletCollisionGroup( mBulletProadphaseAlg.get(), mBulletCollisionConfiguration.get())) { - assert(mCollisionDetector); + // Do nothing } //============================================================================== @@ -117,7 +117,7 @@ void BulletCollisionGroup::notifyAllCollisionObjectsRemoved() } //============================================================================== -void BulletCollisionGroup::updateEngineData() +void BulletCollisionGroup::updateCollisionGroupEngineData() { mBulletCollisionWorld->updateAabbs(); } diff --git a/dart/collision/bullet/BulletCollisionGroup.h b/dart/collision/bullet/BulletCollisionGroup.h index 8342c2cbe32bc..f6f09b3af4f84 100644 --- a/dart/collision/bullet/BulletCollisionGroup.h +++ b/dart/collision/bullet/BulletCollisionGroup.h @@ -75,7 +75,7 @@ class BulletCollisionGroup : public CollisionGroup void notifyAllCollisionObjectsRemoved() override; // Documentation inherited - void updateEngineData() override; + void updateCollisionGroupEngineData() override; /// Return Bullet collision world btCollisionWorld* getBulletCollisionWorld() const; diff --git a/dart/collision/dart/DARTCollisionGroup.cpp b/dart/collision/dart/DARTCollisionGroup.cpp index 299ee1d930a31..cd6341f0da85f 100644 --- a/dart/collision/dart/DARTCollisionGroup.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -46,7 +46,7 @@ DARTCollisionGroup::DARTCollisionGroup( const CollisionDetectorPtr& collisionDetector) : CollisionGroup(collisionDetector) { - assert(mCollisionDetector); + // Do nothing } //============================================================================== @@ -88,7 +88,7 @@ void DARTCollisionGroup::notifyAllCollisionObjectsRemoved() } //============================================================================== -void DARTCollisionGroup::updateEngineData() +void DARTCollisionGroup::updateCollisionGroupEngineData() { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionGroup.h b/dart/collision/dart/DARTCollisionGroup.h index 2761e539d2d48..d50f44d7d7e8e 100644 --- a/dart/collision/dart/DARTCollisionGroup.h +++ b/dart/collision/dart/DARTCollisionGroup.h @@ -73,7 +73,7 @@ class DARTCollisionGroup : public CollisionGroup void notifyAllCollisionObjectsRemoved() override; // Documentation inherited - void updateEngineData() override; + void updateCollisionGroupEngineData() override; }; diff --git a/dart/collision/detail/CollisionDetector.h b/dart/collision/detail/CollisionDetector.h index 3588b72214ee6..099e46d364abf 100644 --- a/dart/collision/detail/CollisionDetector.h +++ b/dart/collision/detail/CollisionDetector.h @@ -51,7 +51,7 @@ std::shared_ptr CollisionDetector::createCollisionGroup( { auto group = createCollisionGroup(); - group->registerShapeFrames(args...); + group->addShapeFramesOf(args...); return group; } diff --git a/dart/collision/detail/CollisionGroup.h b/dart/collision/detail/CollisionGroup.h index 1fcd5bd4d6e70..418a331ab5625 100644 --- a/dart/collision/detail/CollisionGroup.h +++ b/dart/collision/detail/CollisionGroup.h @@ -39,25 +39,150 @@ #include "dart/collision/CollisionGroup.h" +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" + namespace dart { namespace collision { //============================================================================== -template -void CollisionGroup::registerShapeFrames( - const First* first, const Others*... others) +template +void CollisionGroup::addShapeFramesOf( + const dynamics::ShapeFrame* shapeFrame, const Others*... others) +{ + addShapeFrame(shapeFrame); + + addShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::addShapeFramesOf( + const std::vector& shapeFrames, + const Others*... others) +{ + addShapeFrames(shapeFrames); + + addShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::addShapeFramesOf( + const CollisionGroup* otherGroup, const Others*... others) +{ + assert(otherGroup); + + if (otherGroup && this != otherGroup) + { + for (const auto& shapeFrame : otherGroup->mShapeFrames) + addShapeFrame(shapeFrame); + } + + addShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::addShapeFramesOf( + const dynamics::BodyNode* bodyNode, const Others*... others) +{ + assert(bodyNode); + + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); + + for (auto& shapeNode : collisionShapeNodes) + addShapeFrame(shapeNode); + + addShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::addShapeFramesOf( + const dynamics::Skeleton* skel, const Others*... others) +{ + assert(skel); + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + addShapeFramesOf(skel->getBodyNode(i)); + + addShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::removeShapeFramesOf( + const dynamics::ShapeFrame* shapeFrame, const Others*... others) +{ + removeShapeFrame(shapeFrame); + + removeShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::removeShapeFramesOf( + const std::vector& shapeFrames, + const Others*... others) +{ + removeShapeFrames(shapeFrames); + + removeShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::removeShapeFramesOf( + const CollisionGroup* otherGroup, const Others*... others) { - registerShapeFramesOf(first); - registerShapeFrames(others...); + assert(otherGroup); + + if (otherGroup) + { + if (this == otherGroup) + { + unregisterAllShapeFrames(); + return; + } + + for (const auto& shapeFrame : otherGroup->mShapeFrames) + removeShapeFrame(shapeFrame); + } + + removeShapeFramesOf(others...); } //============================================================================== -template -void CollisionGroup::unregisterShapeFrames( - const First* first, const Others*... others) +template +void CollisionGroup::removeShapeFramesOf( + const dynamics::BodyNode* bodyNode, const Others*... others) { - unregisterShapeFramesOf(first); - unregisterShapeFrames(others...); + assert(bodyNode); + + auto collisionShapeNodes + = bodyNode->getShapeNodesWith(); + + for (auto& shapeNode : collisionShapeNodes) + removeShapeFrame(shapeNode); + + removeShapeFramesOf(others...); +} + +//============================================================================== +template +void CollisionGroup::removeShapeFramesOf( + const dynamics::Skeleton* skel, const Others*... others) +{ + assert(skel); + + auto numBodyNodes = skel->getNumBodyNodes(); + for (auto i = 0u; i < numBodyNodes; ++i) + removeShapeFramesOf(skel->getBodyNode(i)); + + removeShapeFramesOf(others...); } } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index d139d798b5028..a5a64f147f4b2 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -765,7 +765,7 @@ bool FCLCollisionDetector::detect( return false; } - group->update(); + group->updateEngineData(); auto castedData = static_cast(group); auto broadPhaseAlg = castedData->getFCLCollisionManager(); @@ -800,8 +800,8 @@ bool FCLCollisionDetector::detect( return false; } - group1->update(); - group2->update(); + group1->updateEngineData(); + group2->updateEngineData(); auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index d66ae91627e1d..0a8a15330c661 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -48,7 +48,7 @@ FCLCollisionGroup::FCLCollisionGroup( : CollisionGroup(collisionDetector), mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) { - assert(mCollisionDetector); + // Do nothing } //============================================================================== @@ -105,7 +105,7 @@ void FCLCollisionGroup::notifyAllCollisionObjectsRemoved() } //============================================================================== -void FCLCollisionGroup::updateEngineData() +void FCLCollisionGroup::updateCollisionGroupEngineData() { mBroadPhaseAlg->update(); } diff --git a/dart/collision/fcl/FCLCollisionGroup.h b/dart/collision/fcl/FCLCollisionGroup.h index 6cfd7d465d9fc..3636446bfd8b2 100644 --- a/dart/collision/fcl/FCLCollisionGroup.h +++ b/dart/collision/fcl/FCLCollisionGroup.h @@ -80,7 +80,7 @@ class FCLCollisionGroup : public CollisionGroup void notifyAllCollisionObjectsRemoved() override; // Documentation inherited - void updateEngineData() override; + void updateCollisionGroupEngineData() override; /// Return FCL collision manager that is also a broad-phase algorithm FCLCollisionManager* getFCLCollisionManager(); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 24f58c3491844..34f070f832de9 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -99,7 +99,7 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) if (!containSkeleton(skeleton)) { auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->registerShapeFramesOf(group.get()); + mCollisionGroup->addShapeFramesOf(group.get()); mSkeletons.push_back(skeleton); mConstrainedGroups.reserve(mSkeletons.size()); @@ -128,7 +128,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) if (containSkeleton(skeleton)) { auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->unregisterShapeFramesOf(group.get()); + mCollisionGroup->removeShapeFramesOf(group.get()); mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), mSkeletons.end()); @@ -231,7 +231,7 @@ void ConstraintSolver::setCollisionDetector( auto newCollisionGroup = mCollisionDetector->createCollisionGroup(); for (const auto& skeleton : mSkeletons) - newCollisionGroup->registerShapeFramesOf(skeleton.get()); + newCollisionGroup->addShapeFramesOf(skeleton.get()); mCollisionGroup = newCollisionGroup; } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index b40d9dce86990..22ce66ccb6e63 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -731,7 +731,7 @@ void testOptions(const std::shared_ptr& cd) EXPECT_TRUE(group->detect(option, result)); EXPECT_EQ(result.getNumContacts(), 2u); - group->registerShapeFrame(simpleFrame3.get()); + group->addShapeFrame(simpleFrame3.get()); result.clear(); option.maxNumContacts = 1e+3; option.binaryCheck = true; From 3f4921190d9476d63458fd12e6e709e1e462264a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 30 Mar 2016 21:28:29 -0400 Subject: [PATCH 49/67] Create CollisionGroup as unique_ptr by default --- dart/collision/CollisionDetector.cpp | 7 +++++ dart/collision/CollisionDetector.h | 27 +++++++++++++++---- dart/collision/CollisionGroup.h | 6 ++--- .../bullet/BulletCollisionDetector.cpp | 5 ++-- .../bullet/BulletCollisionDetector.h | 2 +- dart/collision/dart/DARTCollisionDetector.cpp | 5 ++-- dart/collision/dart/DARTCollisionDetector.h | 2 +- dart/collision/detail/CollisionDetector.h | 13 +++++++-- dart/collision/fcl/FCLCollisionDetector.cpp | 5 ++-- dart/collision/fcl/FCLCollisionDetector.h | 2 +- dart/constraint/ConstraintSolver.cpp | 2 +- dart/constraint/ConstraintSolver.h | 2 +- 12 files changed, 56 insertions(+), 22 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index cc417bd2be449..ab063c3abc864 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -48,6 +48,13 @@ namespace dart { namespace collision { +//============================================================================== +std::shared_ptr +CollisionDetector::createCollisionGroupAsSharedPtr() +{ + return std::shared_ptr(createCollisionGroup().release()); +} + //============================================================================== CollisionObject* CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 8a95569210eb1..52b6ab78b0bad 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -65,14 +65,31 @@ class CollisionDetector : public std::enable_shared_from_this virtual const std::string& getType() const = 0; /// Create a collision group - virtual std::shared_ptr createCollisionGroup() = 0; + virtual std::unique_ptr createCollisionGroup() = 0; + + /// Helper function that creates and returns CollisionGroup as shared_ptr. + /// + /// Internally, this function creates shared_ptr from unique_ptr returned from + /// createCollisionGroup() so the performance would be slighly worse than + /// using std::make_unique. + std::shared_ptr createCollisionGroupAsSharedPtr(); /// Create a collision group from any objects that are supported by - /// CollisionGroup::addShapeFramesOf(). Currently, the supporting objects - /// are ShapeFrame, std::vector, CollisionGroup, BodyNode, and - /// Skeleton. + /// CollisionGroup::addShapeFramesOf(). + /// + /// The objects can be any of ShapeFrame, std::vector, + /// CollisionGroup, BodyNode, and Skeleton. + /// + /// Note that this function adds only the ShapeFrames of each object at the + /// moment of this function is called. The aftwerward changes of the objects + /// does not affect on this CollisionGroup. + template + std::unique_ptr createCollisionGroup(const Args&... args); + + /// Helper function that creates and returns CollisionGroup as shared_ptr. template - std::shared_ptr createCollisionGroup(const Args&... args); + std::shared_ptr createCollisionGroupAsSharedPtr( + const Args&... args); /// Perform collision detection for group. virtual bool detect(CollisionGroup* group, diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 7b87d0cf20c40..706659d47588d 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -76,8 +76,7 @@ class CollisionGroup /// Add a ShapeFrame, and also add ShapeFrames of other various objects. /// /// The other various objects can be any of ShapeFrame, - /// std::vector, CollisionGroup, BodyNode, Skeleton, and - /// CollisionGroup. + /// std::vector, CollisionGroup, BodyNode, and Skeleton. /// /// Note that this function adds only the ShapeFrames of each object at the /// moment of this function is called. The aftwerward changes of the objects @@ -124,8 +123,7 @@ class CollisionGroup /// Remove a ShapeFrame, and also remove ShapeFrames of other various objects. /// /// The other various objects can be any of ShapeFrame, - /// std::vector, CollisionGroup, BodyNode, Skeleton, and - /// CollisionGroup. + /// std::vector, CollisionGroup, BodyNode, and Skeleton. /// /// Note that this function removes only the ShapeFrames of each object at the /// moment of this function is called. The aftwerward changes of the objects diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 23e09bc5eb0e3..66fbca01a38e0 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -166,9 +166,10 @@ const std::string& BulletCollisionDetector::getType() const } //============================================================================== -std::shared_ptr BulletCollisionDetector::createCollisionGroup() +std::unique_ptr +BulletCollisionDetector::createCollisionGroup() { - return std::make_shared(shared_from_this()); + return common::make_unique(shared_from_this()); } //============================================================================== diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 342018f887793..ffafaa151e12f 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -71,7 +71,7 @@ class BulletCollisionDetector : public CollisionDetector const std::string& getType() const override; // Documentation inherited - std::shared_ptr createCollisionGroup() override; + std::unique_ptr createCollisionGroup() override; // Documentation inherited bool detect(CollisionGroup* group, diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 15de42c61bf50..c6a9cb0841f8d 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -82,9 +82,10 @@ const std::string& DARTCollisionDetector::getType() const } //============================================================================== -std::shared_ptr DARTCollisionDetector::createCollisionGroup() +std::unique_ptr +DARTCollisionDetector::createCollisionGroup() { - return std::make_shared(shared_from_this()); + return common::make_unique(shared_from_this()); } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 00c2aead345ca..df7593896b086 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -58,7 +58,7 @@ class DARTCollisionDetector : public CollisionDetector const std::string& getType() const override; // Documentation inherited - std::shared_ptr createCollisionGroup() override; + std::unique_ptr createCollisionGroup() override; // Documentation inherited bool detect(CollisionGroup* group, diff --git a/dart/collision/detail/CollisionDetector.h b/dart/collision/detail/CollisionDetector.h index 099e46d364abf..fc24d30f20663 100644 --- a/dart/collision/detail/CollisionDetector.h +++ b/dart/collision/detail/CollisionDetector.h @@ -46,8 +46,8 @@ namespace collision { //============================================================================== template -std::shared_ptr CollisionDetector::createCollisionGroup( - const Args&... args) +std::unique_ptr +CollisionDetector::createCollisionGroup(const Args&... args) { auto group = createCollisionGroup(); @@ -56,6 +56,15 @@ std::shared_ptr CollisionDetector::createCollisionGroup( return group; } +//============================================================================== +template +std::shared_ptr +CollisionDetector::createCollisionGroupAsSharedPtr(const Args&... args) +{ + return std::shared_ptr( + createCollisionGroup(args...).release()); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index a5a64f147f4b2..cda4c1f75bfaf 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -745,9 +745,10 @@ const std::string& FCLCollisionDetector::getType() const } //============================================================================== -std::shared_ptr FCLCollisionDetector::createCollisionGroup() +std::unique_ptr +FCLCollisionDetector::createCollisionGroup() { - return std::make_shared(shared_from_this()); + return common::make_unique(shared_from_this()); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 215ff4f318394..d9cb49576e1d5 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -63,7 +63,7 @@ class FCLCollisionDetector : public CollisionDetector const std::string& getType() const override; // Documentation inherited - std::shared_ptr createCollisionGroup() override; + std::unique_ptr createCollisionGroup() override; // Documentation inherited bool detect(CollisionGroup* group, diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 34f070f832de9..0432a2951ae53 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -233,7 +233,7 @@ void ConstraintSolver::setCollisionDetector( for (const auto& skeleton : mSkeletons) newCollisionGroup->addShapeFramesOf(skeleton.get()); - mCollisionGroup = newCollisionGroup; + mCollisionGroup = std::move(newCollisionGroup); } //============================================================================== diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index 852e1c26db716..da63178eb9676 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -173,7 +173,7 @@ class ConstraintSolver std::shared_ptr mCollisionDetector; /// Collision group - std::shared_ptr mCollisionGroup; + std::unique_ptr mCollisionGroup; /// Last collision checking result collision::Option mCollisionOption; From 7a5863c18084e3b5e12a01877932a486458e2c0d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 30 Mar 2016 22:38:08 -0400 Subject: [PATCH 50/67] Improve CollisionObject ownership management using RAII -- BulletCollisionDetector causes segfaults --- dart/collision/CollisionDetector.cpp | 137 ++++-------------- dart/collision/CollisionDetector.h | 104 +++---------- dart/collision/CollisionGroup.cpp | 20 +-- dart/collision/CollisionGroup.h | 8 +- .../bullet/BulletCollisionDetector.cpp | 11 +- .../bullet/BulletCollisionDetector.h | 4 +- .../collision/bullet/BulletCollisionGroup.cpp | 4 +- dart/collision/dart/DARTCollisionDetector.cpp | 18 +-- dart/collision/dart/DARTCollisionDetector.h | 4 +- dart/collision/dart/DARTCollisionGroup.cpp | 2 +- dart/collision/detail/CollisionGroup.h | 2 +- dart/collision/fcl/FCLCollisionDetector.cpp | 7 +- dart/collision/fcl/FCLCollisionDetector.h | 2 +- dart/collision/fcl/FCLCollisionGroup.cpp | 2 +- dart/constraint/ConstraintSolver.cpp | 2 +- unittests/testCollision.cpp | 4 +- 16 files changed, 83 insertions(+), 248 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index ab063c3abc864..93c609b4508df 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -56,144 +56,63 @@ CollisionDetector::createCollisionGroupAsSharedPtr() } //============================================================================== -CollisionObject* CollisionDetector::claimCollisionObject( - const dynamics::ShapeFrame* shapeFrame) -{ - if (!mCollisionObjectManager) - mCollisionObjectManager.reset(new NaiveCollisionObjectManager(this)); - - return mCollisionObjectManager->claimCollisionObject(shapeFrame); -} - -//============================================================================== -void CollisionDetector::reclaimCollisionObject(const CollisionObject* collObj) -{ - if (!mCollisionObjectManager) - mCollisionObjectManager.reset(new NaiveCollisionObjectManager(this)); - - mCollisionObjectManager->reclaimCollisionObject(collObj); -} - -//============================================================================== -CollisionDetector::CollisionObjectManager::CollisionObjectManager( - CollisionDetector* cd) - : mCollisionDetector(cd) -{ - // Do nothing -} - -//============================================================================== -CollisionDetector:: -NaiveCollisionObjectManager::NaiveCollisionObjectManager( - CollisionDetector* cd) - : CollisionDetector::CollisionObjectManager(cd) +CollisionDetector::CollisionDetector() + : mCollisionObjectDeleter(this) { // Do nothing } //============================================================================== -CollisionDetector:: -NaiveCollisionObjectManager::~NaiveCollisionObjectManager() +CollisionDetector::~CollisionDetector() { - assert(mCollisionObjects.empty()); + assert(mCollisionObjectMap.empty()); } //============================================================================== -CollisionObject* -CollisionDetector::NaiveCollisionObjectManager::claimCollisionObject( +std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) { - mCollisionObjects.push_back( - std::move(mCollisionDetector->createCollisionObject(shapeFrame))); + auto search = mCollisionObjectMap.find(shapeFrame); - return mCollisionObjects.back().get(); -} + if (mCollisionObjectMap.end() != search) + { + const auto& collObj = search->second; + assert(collObj.lock()); -//============================================================================== -void CollisionDetector::NaiveCollisionObjectManager::reclaimCollisionObject( - const CollisionObject* object) -{ - auto search = std::find_if(mCollisionObjects.begin(), mCollisionObjects.end(), - [&](const std::unique_ptr& it) - { return it.get() == object; }); + return collObj.lock(); + } - if (mCollisionObjects.end() == search) - return; + auto newObject = std::shared_ptr( + createCollisionObject(shapeFrame), mCollisionObjectDeleter); - mCollisionDetector->notifyCollisionObjectDestorying(search->get()); - mCollisionObjects.erase(search); -} + mCollisionObjectMap[shapeFrame] = newObject; -//============================================================================== -CollisionDetector:: -RefCountingCollisionObjectManager::RefCountingCollisionObjectManager( - CollisionDetector* cd) - : CollisionDetector::CollisionObjectManager(cd) -{ - // Do nothing + return newObject; } //============================================================================== -CollisionDetector:: -RefCountingCollisionObjectManager::~RefCountingCollisionObjectManager() +void CollisionDetector::reclaimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { - assert(mCollisionObjectMap.empty()); + mCollisionObjectMap.erase(shapeFrame); } //============================================================================== -CollisionObject* -CollisionDetector::RefCountingCollisionObjectManager::claimCollisionObject( - const dynamics::ShapeFrame* shapeFrame) +CollisionDetector::CollisionObjectDeletor::CollisionObjectDeletor( + CollisionDetector* cd) + : mCollisionDetector(cd) { - auto search = mCollisionObjectMap.find(shapeFrame); - - // Found existing collision object - if (mCollisionObjectMap.end() != search) - { - auto& collObjAndCount = search->second; - - auto& collObj = collObjAndCount.first; - auto& count = collObjAndCount.second; - assert(count != 0u); - - count++; - - return collObj.get(); - } - - auto newCollisionObject - = mCollisionDetector->createCollisionObject(shapeFrame); - - mCollisionObjectMap[shapeFrame] - = std::make_pair(std::move(newCollisionObject), 1u); - - return mCollisionObjectMap[shapeFrame].first.get(); + // Do nothing } //============================================================================== -void CollisionDetector:: -RefCountingCollisionObjectManager::reclaimCollisionObject( - const CollisionObject* object) +void CollisionDetector::CollisionObjectDeletor::operator()( + CollisionObject* object) const { - auto shapeFrame = object->getShapeFrame(); - auto search = mCollisionObjectMap.find(shapeFrame); - - if (mCollisionObjectMap.end() == search) - return; - - auto& collObjAndCount = search->second; - auto& count = collObjAndCount.second; - assert(count != 0u); - - count--; + mCollisionDetector->notifyCollisionObjectDestorying(object); + mCollisionDetector->reclaimCollisionObject(object->getShapeFrame()); - if (0u == count) - { - auto& collisionObject = collObjAndCount.first; - mCollisionDetector->notifyCollisionObjectDestorying(collisionObject.get()); - - mCollisionObjectMap.erase(search); - } + delete object; } } // namespace collision diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 52b6ab78b0bad..74b07d9c2c1da 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -61,6 +61,9 @@ class CollisionDetector : public std::enable_shared_from_this friend class CollisionObject; friend class CollisionGroup; + /// Destructor + virtual ~CollisionDetector(); + /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; @@ -101,25 +104,16 @@ class CollisionDetector : public std::enable_shared_from_this protected: - // We define following collision object managers outside of this class for - // better code readability. - class CollisionObjectManager; - class NaiveCollisionObjectManager; - class RefCountingCollisionObjectManager; - /// Constructor - CollisionDetector() = default; + CollisionDetector(); /// Claim CollisionObject associated with shapeFrame. New CollisionObject /// will be created if it hasn't created yet for shapeFrame. - CollisionObject* claimCollisionObject(const dynamics::ShapeFrame* shapeFrame); - - /// Reclaim a CollisionObject. The CollisionObject will be destroyed if no - /// CollisionGroup holds it. - void reclaimCollisionObject(const CollisionObject* collObj); + std::shared_ptr claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame); /// Create CollisionObject - virtual std::unique_ptr createCollisionObject( + virtual CollisionObject* createCollisionObject( const dynamics::ShapeFrame* shapeFrame) = 0; /// Notify that a CollisionObject will be destroyed so that the collision @@ -128,84 +122,26 @@ class CollisionDetector : public std::enable_shared_from_this protected: - std::unique_ptr mCollisionObjectManager; - -}; - -class CollisionDetector::CollisionObjectManager -{ -public: - - /// Constructor - CollisionObjectManager(CollisionDetector* cd); - - /// Return CollisionObject associated with shapeFrame. New CollisionObject - /// will be created if it hasn't created yet for shapeFrame. - virtual CollisionObject* claimCollisionObject( - const dynamics::ShapeFrame* shapeFrame) = 0; - - /// Reclaim CollisionObject associated with shapeFrame. The CollisionObject - /// will be destroyed if no CollisionGroup holds it. - virtual void reclaimCollisionObject(const CollisionObject* shapeFrame) = 0; - -protected: - - CollisionDetector* mCollisionDetector; - -}; - -class CollisionDetector::NaiveCollisionObjectManager : - public CollisionDetector::CollisionObjectManager -{ -public: - - /// Constructor - NaiveCollisionObjectManager(CollisionDetector* cd); - - /// Destructor - virtual ~NaiveCollisionObjectManager(); - - // Documentation inherited - CollisionObject* claimCollisionObject( - const dynamics::ShapeFrame* shapeFrame) override; - - // Documentation inherited - void reclaimCollisionObject(const CollisionObject* shapeFrame) override; - -protected: - - std::vector> mCollisionObjects; - -}; - -class CollisionDetector::RefCountingCollisionObjectManager : - public CollisionDetector::CollisionObjectManager -{ -public: + using CollisionObjectMap + = std::map>; - /// Constructor - RefCountingCollisionObjectManager(CollisionDetector* cd); + CollisionObjectMap mCollisionObjectMap; - /// Destructor - virtual ~RefCountingCollisionObjectManager(); +private: - // Documentation inherited - CollisionObject* claimCollisionObject( - const dynamics::ShapeFrame* shapeFrame) override; + /// Reclaim a CollisionObject associated with given ShapeFrame. + void reclaimCollisionObject(const dynamics::ShapeFrame* shapeFrame); - // Documentation inherited - void reclaimCollisionObject(const CollisionObject* shapeFrame) override; + struct CollisionObjectDeletor final + { + CollisionDetector* mCollisionDetector; -protected: + CollisionObjectDeletor(CollisionDetector* cd); - using CollisionObjectMapValue - = std::pair, size_t>; - using CollisionObjectMap - = std::map; - using CollisionObjectPair - = std::pair; + void operator()(CollisionObject* object) const; + }; - CollisionObjectMap mCollisionObjectMap; + CollisionObjectDeletor mCollisionObjectDeleter; }; diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 4f08073d738a5..1e4ac7e4e4eaf 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -56,8 +56,7 @@ CollisionGroup::CollisionGroup(const CollisionDetectorPtr& collisionDetector) //============================================================================== CollisionGroup::~CollisionGroup() { - assert(mShapeFrames.empty()); - assert(mCollisionObjects.empty()); + // Do nothing } //============================================================================== @@ -83,7 +82,7 @@ void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame); - notifyCollisionObjectAdded(collObj); + notifyCollisionObjectAdded(collObj.get()); mShapeFrames.push_back(shapeFrame); mCollisionObjects.push_back(collObj); @@ -104,8 +103,7 @@ void CollisionGroup::addShapeFramesOf() } //============================================================================== -void CollisionGroup::removeShapeFrame( - const dynamics::ShapeFrame* shapeFrame) +void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) { if (!shapeFrame) return; @@ -120,9 +118,7 @@ void CollisionGroup::removeShapeFrame( const size_t index = search - mShapeFrames.begin(); - notifyCollisionObjectRemoved(mCollisionObjects[index]); - - mCollisionDetector->reclaimCollisionObject(mCollisionObjects[index]); + notifyCollisionObjectRemoved(mCollisionObjects[index].get()); mShapeFrames.erase(search); mCollisionObjects.erase(mCollisionObjects.begin() + index); @@ -143,13 +139,10 @@ void CollisionGroup::removeShapeFramesOf() } //============================================================================== -void CollisionGroup::unregisterAllShapeFrames() +void CollisionGroup::removeAllShapeFrames() { notifyAllCollisionObjectsRemoved(); - for (const auto& object : mCollisionObjects) - mCollisionDetector->reclaimCollisionObject(object); - mShapeFrames.clear(); mCollisionObjects.clear(); } @@ -197,7 +190,8 @@ bool CollisionGroup::detect( } //============================================================================== -const std::vector& CollisionGroup::getCollisionObjects() +const std::vector>& +CollisionGroup::getCollisionObjects() { return mCollisionObjects; } diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 706659d47588d..1963d30c40773 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -160,8 +160,8 @@ class CollisionGroup /// template function template removeShapeFramesOf(). void removeShapeFramesOf(); - /// Unregister all the ShapeFrames in this CollisionGroup - void unregisterAllShapeFrames(); + /// Remove all the ShapeFrames in this CollisionGroup + void removeAllShapeFrames(); /// Return true if this CollisionGroup contains shapeFrame bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const; @@ -186,7 +186,7 @@ class CollisionGroup bool detect(CollisionGroup* group, const Option& option, Result& result); /// Return all the CollisionObjects in this CollisionGroup - const std::vector& getCollisionObjects(); + const std::vector >& getCollisionObjects(); protected: @@ -229,7 +229,7 @@ class CollisionGroup std::vector mShapeFrames; /// CollisionObjects associated with the registered ShapeFrames - std::vector mCollisionObjects; + std::vector> mCollisionObjects; }; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 66fbca01a38e0..b9fd434350a96 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -241,14 +241,7 @@ bool BulletCollisionDetector::detect( } //============================================================================== -BulletCollisionDetector::BulletCollisionDetector() -{ - mCollisionObjectManager.reset(new NaiveCollisionObjectManager(this)); -} - -//============================================================================== -std::unique_ptr -BulletCollisionDetector::createCollisionObject( +CollisionObject* BulletCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { auto pack = claimBulletCollisionGeometry(shapeFrame->getShape()); @@ -258,7 +251,7 @@ BulletCollisionDetector::createCollisionObject( mBulletCollisionObjectMap[object] = collObj; - return std::unique_ptr(collObj); + return collObj; } //============================================================================== diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index ffafaa151e12f..a2f0d9c8be1d5 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -84,10 +84,10 @@ class BulletCollisionDetector : public CollisionDetector protected: /// Constructor - BulletCollisionDetector(); + BulletCollisionDetector() = default; // Documentation inherited - std::unique_ptr createCollisionObject( + CollisionObject* createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; // Documentation inherited diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 0ad23c845f9b5..bb4c9a19d154d 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -61,7 +61,7 @@ BulletCollisionGroup::BulletCollisionGroup( //============================================================================== BulletCollisionGroup::~BulletCollisionGroup() { - unregisterAllShapeFrames(); + // Do nothing } //============================================================================== @@ -111,7 +111,7 @@ void BulletCollisionGroup::notifyCollisionObjectRemoved( void BulletCollisionGroup::notifyAllCollisionObjectsRemoved() { for (const auto& collisionObject : getCollisionObjects()) - notifyCollisionObjectRemoved(collisionObject); + notifyCollisionObjectRemoved(collisionObject.get()); initializeEngineData(); } diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index c6a9cb0841f8d..1f71ec1104c88 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -115,10 +115,10 @@ bool DARTCollisionDetector::detect( { auto collObj2 = objects[j]; - if (filter && !filter->needCollision(collObj1, collObj2)) + if (filter && !filter->needCollision(collObj1.get(), collObj2.get())) continue; - checkPair(collObj1, collObj2, option, result); + checkPair(collObj1.get(), collObj2.get(), option, result); if ((option.binaryCheck && result.isCollision()) || (result.getNumContacts() >= option.maxNumContacts)) @@ -167,10 +167,10 @@ bool DARTCollisionDetector::detect( { auto collObj2 = objects2[j]; - if (filter && !filter->needCollision(collObj1, collObj2)) + if (filter && !filter->needCollision(collObj1.get(), collObj2.get())) continue; - checkPair(collObj1, collObj2, option, result); + checkPair(collObj1.get(), collObj2.get(), option, result); if (result.getNumContacts() >= option.maxNumContacts) { @@ -186,12 +186,6 @@ bool DARTCollisionDetector::detect( return result.isCollision(); } -//============================================================================== -DARTCollisionDetector::DARTCollisionDetector() -{ - mCollisionObjectManager.reset(new RefCountingCollisionObjectManager(this)); -} - //============================================================================== void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) { @@ -220,7 +214,7 @@ void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) } //============================================================================== -std::unique_ptr DARTCollisionDetector::createCollisionObject( +CollisionObject* DARTCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { auto collObj = new DARTCollisionObject(this, shapeFrame); @@ -229,7 +223,7 @@ std::unique_ptr DARTCollisionDetector::createCollisionObject( mDARTCollisionObjects.push_back(collObj); - return std::unique_ptr(collObj); + return collObj; } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index df7593896b086..fa41b43414ad0 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -71,10 +71,10 @@ class DARTCollisionDetector : public CollisionDetector protected: /// Constructor - DARTCollisionDetector(); + DARTCollisionDetector() = default; // Documentation inherited - std::unique_ptr createCollisionObject( + CollisionObject* createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; // Documentation inherited diff --git a/dart/collision/dart/DARTCollisionGroup.cpp b/dart/collision/dart/DARTCollisionGroup.cpp index cd6341f0da85f..944ceaf292770 100644 --- a/dart/collision/dart/DARTCollisionGroup.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -52,7 +52,7 @@ DARTCollisionGroup::DARTCollisionGroup( //============================================================================== DARTCollisionGroup::~DARTCollisionGroup() { - unregisterAllShapeFrames(); + // Do nothing } //============================================================================== diff --git a/dart/collision/detail/CollisionGroup.h b/dart/collision/detail/CollisionGroup.h index 418a331ab5625..81efad033f4df 100644 --- a/dart/collision/detail/CollisionGroup.h +++ b/dart/collision/detail/CollisionGroup.h @@ -144,7 +144,7 @@ void CollisionGroup::removeShapeFramesOf( { if (this == otherGroup) { - unregisterAllShapeFrames(); + removeAllShapeFrames(); return; } diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index cda4c1f75bfaf..5b3f17d0cd2f5 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -872,12 +872,11 @@ FCLCollisionDetector::FCLCollisionDetector() : mPrimitiveShapeType(MESH), mContactPointComputationMethod(DART) { - mCollisionObjectManager.reset(new RefCountingCollisionObjectManager(this)); + // Do nothing } //============================================================================== -std::unique_ptr -FCLCollisionDetector::createCollisionObject( +CollisionObject* FCLCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { auto fclCollGeom = claimFCLCollisionGeometry(shapeFrame->getShape()); @@ -886,7 +885,7 @@ FCLCollisionDetector::createCollisionObject( mFCLCollisionObjectMap[fclCollObj] = collObj; - return std::unique_ptr(collObj); + return collObj; } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index d9cb49576e1d5..3a401a4a6ff5a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -110,7 +110,7 @@ class FCLCollisionDetector : public CollisionDetector FCLCollisionDetector(); // Documentation inherited - std::unique_ptr createCollisionObject( + CollisionObject* createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; // Documentation inherited diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index 0a8a15330c661..72df8f35b652d 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -54,7 +54,7 @@ FCLCollisionGroup::FCLCollisionGroup( //============================================================================== FCLCollisionGroup::~FCLCollisionGroup() { - unregisterAllShapeFrames(); + // Do nothing } //============================================================================== diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 0432a2951ae53..4ada32e6d7038 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -153,7 +153,7 @@ void ConstraintSolver::removeSkeletons( //============================================================================== void ConstraintSolver::removeAllSkeletons() { - mCollisionGroup->unregisterAllShapeFrames(); + mCollisionGroup->removeAllShapeFrames(); mSkeletons.clear(); } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 22ce66ccb6e63..3c3800298916d 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -585,8 +585,8 @@ TEST_F(COLLISION, SimpleFrames) // testSimpleFrames(fcl_mesh_fcl); #if HAVE_BULLET_COLLISION - auto bullet = BulletCollisionDetector::create(); - testSimpleFrames(bullet); +// auto bullet = BulletCollisionDetector::create(); +// testSimpleFrames(bullet); #endif auto dart = DARTCollisionDetector::create(); From 1c8e36fb61ee2e835bf8722035ef2c667f43ffbe Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 4 Apr 2016 14:06:12 -0400 Subject: [PATCH 51/67] Improve fcl::CollisionGeometry/btCollisionShape ownership management using RAII -- BulletCollisionDetector still causes segfaults --- dart/collision/CollisionDetector.cpp | 26 +- dart/collision/CollisionDetector.h | 31 +- dart/collision/CollisionGroup.h | 11 +- .../bullet/BulletCollisionDetector.cpp | 181 ++++----- .../bullet/BulletCollisionDetector.h | 43 +- dart/collision/dart/DARTCollisionDetector.cpp | 21 +- dart/collision/dart/DARTCollisionDetector.h | 9 +- dart/collision/fcl/FCLCollisionDetector.cpp | 368 +++++++++--------- dart/collision/fcl/FCLCollisionDetector.h | 101 +++-- unittests/testCollision.cpp | 4 +- 10 files changed, 381 insertions(+), 414 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 93c609b4508df..9222e898ed828 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -72,33 +72,28 @@ CollisionDetector::~CollisionDetector() std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) { - auto search = mCollisionObjectMap.find(shapeFrame); + const auto search = mCollisionObjectMap.find(shapeFrame); if (mCollisionObjectMap.end() != search) { const auto& collObj = search->second; assert(collObj.lock()); + // Ensure all the collision object in the map should be alive pointers. return collObj.lock(); } - auto newObject = std::shared_ptr( - createCollisionObject(shapeFrame), mCollisionObjectDeleter); + auto uniqueObject = createCollisionObject(shapeFrame); + auto sharedObject = std::shared_ptr( + uniqueObject.release(), mCollisionObjectDeleter); - mCollisionObjectMap[shapeFrame] = newObject; + mCollisionObjectMap[shapeFrame] = sharedObject; - return newObject; + return sharedObject; } //============================================================================== -void CollisionDetector::reclaimCollisionObject( - const dynamics::ShapeFrame* shapeFrame) -{ - mCollisionObjectMap.erase(shapeFrame); -} - -//============================================================================== -CollisionDetector::CollisionObjectDeletor::CollisionObjectDeletor( +CollisionDetector::CollisionObjectDeleter::CollisionObjectDeleter( CollisionDetector* cd) : mCollisionDetector(cd) { @@ -106,11 +101,10 @@ CollisionDetector::CollisionObjectDeletor::CollisionObjectDeletor( } //============================================================================== -void CollisionDetector::CollisionObjectDeletor::operator()( +void CollisionDetector::CollisionObjectDeleter::operator()( CollisionObject* object) const { - mCollisionDetector->notifyCollisionObjectDestorying(object); - mCollisionDetector->reclaimCollisionObject(object->getShapeFrame()); + mCollisionDetector->mCollisionObjectMap.erase(object->getShapeFrame()); delete object; } diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 74b07d9c2c1da..744110b43d5d4 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -113,35 +113,28 @@ class CollisionDetector : public std::enable_shared_from_this const dynamics::ShapeFrame* shapeFrame); /// Create CollisionObject - virtual CollisionObject* createCollisionObject( + virtual std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) = 0; - /// Notify that a CollisionObject will be destroyed so that the collision - /// detection engine do some relevant work. - virtual void notifyCollisionObjectDestorying(CollisionObject* collObj) = 0; - -protected: - - using CollisionObjectMap - = std::map>; - - CollisionObjectMap mCollisionObjectMap; - private: - - /// Reclaim a CollisionObject associated with given ShapeFrame. - void reclaimCollisionObject(const dynamics::ShapeFrame* shapeFrame); - - struct CollisionObjectDeletor final +\ + /// This deleter is responsible for deleting CollisionObject and removing it + /// from mCollisionObjectMap when it is not shared by any CollisionGroups. + struct CollisionObjectDeleter final { CollisionDetector* mCollisionDetector; - CollisionObjectDeletor(CollisionDetector* cd); + CollisionObjectDeleter(CollisionDetector* cd); void operator()(CollisionObject* object) const; }; - CollisionObjectDeletor mCollisionObjectDeleter; + const CollisionObjectDeleter mCollisionObjectDeleter; + + using CollisionObjectMap = std::map>; + + CollisionObjectMap mCollisionObjectMap; }; diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 1963d30c40773..350154fd1210b 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -55,6 +55,7 @@ class CollisionGroup /// Constructor CollisionGroup(const CollisionDetectorPtr& collisionDetector); + // CollisionGroup also can be created from CollisionDetector::create() /// Destructor virtual ~CollisionGroup(); @@ -224,12 +225,18 @@ class CollisionGroup /// Collision detector CollisionDetectorPtr mCollisionDetector; + // CollisionGroup shares the ownership of CollisionDetector with other + // CollisionGroups created from the same CollisionDetector so that the + // CollisionDetector doesn't get destroyed as long as at least one + // CollisionGroup is alive. - /// ShapeFrames registered to this CollisionGroup + /// ShapeFrames added to this CollisionGroup std::vector mShapeFrames; - /// CollisionObjects associated with the registered ShapeFrames + /// CollisionObjects associated with the added ShapeFrames std::vector> mCollisionObjects; + // CollisionGroup also shares the ownership of CollisionObjects across other + // CollisionGroups for the same reason with above. }; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index b9fd434350a96..c98ffc913dd95 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -125,15 +125,13 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, void convertContacts( btCollisionWorld* collWorld, const Option& option, Result& result); -BulletCollisionDetector::BulletCollsionShapePack +btCollisionShape* createBulletEllipsoidMesh(float sizeX, float sizeY, float sizeZ); -BulletCollisionDetector::BulletCollsionShapePack -createBulletCollisionShapePackFromAssimpScene( +btCollisionShape* createBulletCollisionShapeFromAssimpScene( const Eigen::Vector3d& scale, const aiScene* scene); -BulletCollisionDetector::BulletCollsionShapePack -createBulletCollisionShapePackFromAssimpMesh(const aiMesh* mesh); +btCollisionShape* createBulletCollisionShapeFromAssimpMesh(const aiMesh* mesh); } // anonymous namespace @@ -241,79 +239,43 @@ bool BulletCollisionDetector::detect( } //============================================================================== -CollisionObject* BulletCollisionDetector::createCollisionObject( +std::unique_ptr BulletCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { - auto pack = claimBulletCollisionGeometry(shapeFrame->getShape()); - auto collObj = new BulletCollisionObject(this, shapeFrame, - pack.collisionShape.get()); - auto object = collObj->getBulletCollisionObject(); + auto bulletCollShape = claimBulletCollisionGeometry(shapeFrame->getShape()); - mBulletCollisionObjectMap[object] = collObj; - - return collObj; -} - -//============================================================================== -void BulletCollisionDetector::notifyCollisionObjectDestorying( - CollisionObject* collObj) -{ - if (!collObj) - return; - - reclaimBulletCollisionGeometry(collObj->getShape()); - - auto casted = static_cast(collObj); - mBulletCollisionObjectMap.erase(casted->getBulletCollisionObject()); + return std::unique_ptr( + new BulletCollisionObject(this, shapeFrame, bulletCollShape.get())); } //============================================================================== -BulletCollisionDetector::BulletCollsionShapePack +std::shared_ptr BulletCollisionDetector::claimBulletCollisionGeometry( const dynamics::ConstShapePtr& shape) { - BulletCollsionShapePack pack; + const auto search = mShapeMap.find(shape); - auto findResult = mShapeMap.find(shape); - if (mShapeMap.end() != findResult) + if (mShapeMap.end() != search) { - auto& packAndCount = findResult->second; - - pack = packAndCount.first; + const auto& bulletCollShape = search->second; + assert(bulletCollShape.lock()); + // Ensure all the collision shape in the map should be alive pointers. - auto& count = packAndCount.second; - assert(0u != count); - count++; - } - else - { - pack = createBulletCollisionShapePack(shape); - mShapeMap[shape] = std::make_pair(pack, 1u); + return bulletCollShape.lock(); } - return pack; -} - -//============================================================================== -void BulletCollisionDetector::reclaimBulletCollisionGeometry( - const dynamics::ConstShapePtr& shape) -{ - auto findResult = mShapeMap.find(shape); - assert(mShapeMap.end() != findResult); - - ShapeMapValue& pair = findResult->second; - pair.second--; + auto newBulletCollisionShape = createBulletCollisionShape( + shape, BulletCollisionShapeDeleter(this, shape)); + mShapeMap[shape] = newBulletCollisionShape; - if (0u == pair.second) - { - mShapeMap.erase(findResult); - } + return newBulletCollisionShape; } //============================================================================== -BulletCollisionDetector::BulletCollsionShapePack -BulletCollisionDetector::createBulletCollisionShapePack( - const dynamics::ConstShapePtr& shape) +std::shared_ptr +BulletCollisionDetector::createBulletCollisionShape( + const dynamics::ConstShapePtr& shape, + const BulletCollisionShapeDeleter& deleter) { using dynamics::Shape; using dynamics::BoxShape; @@ -323,9 +285,7 @@ BulletCollisionDetector::createBulletCollisionShapePack( using dynamics::MeshShape; using dynamics::SoftMeshShape; - BulletCollsionShapePack pack; - - auto& bulletCollisionShape = pack.collisionShape; + btCollisionShape* bulletCollisionShape = nullptr; switch (shape->getShapeType()) { @@ -336,7 +296,7 @@ BulletCollisionDetector::createBulletCollisionShapePack( const auto box = static_cast(shape.get()); const Eigen::Vector3d& size = box->getSize(); - bulletCollisionShape.reset(new btBoxShape(convertVector3(size*0.5))); + bulletCollisionShape = new btBoxShape(convertVector3(size*0.5)); break; } @@ -348,9 +308,14 @@ BulletCollisionDetector::createBulletCollisionShapePack( const Eigen::Vector3d& size = ellipsoid->getSize(); if (ellipsoid->isSphere()) - bulletCollisionShape.reset(new btSphereShape(size[0] * 0.5)); + { + bulletCollisionShape = new btSphereShape(size[0] * 0.5); + } else - pack = createBulletEllipsoidMesh(size[0], size[1], size[2]); + { + bulletCollisionShape = createBulletEllipsoidMesh( + size[0], size[1], size[2]); + } break; } @@ -363,7 +328,7 @@ BulletCollisionDetector::createBulletCollisionShapePack( const auto height = cylinder->getHeight(); const auto size = btVector3(radius, radius, height * 0.5); - bulletCollisionShape.reset(new btCylinderShapeZ(size)); + bulletCollisionShape = new btCylinderShapeZ(size); break; } @@ -375,8 +340,8 @@ BulletCollisionDetector::createBulletCollisionShapePack( const Eigen::Vector3d normal = plane->getNormal(); const double offset = plane->getOffset(); - bulletCollisionShape.reset(new btStaticPlaneShape( - convertVector3(normal), offset)); + bulletCollisionShape = new btStaticPlaneShape( + convertVector3(normal), offset); break; } @@ -388,7 +353,8 @@ BulletCollisionDetector::createBulletCollisionShapePack( const auto scale = shapeMesh->getScale(); const auto mesh = shapeMesh->getMesh(); - pack = createBulletCollisionShapePackFromAssimpScene(scale, mesh); + bulletCollisionShape = createBulletCollisionShapeFromAssimpScene( + scale, mesh); break; } @@ -399,7 +365,7 @@ BulletCollisionDetector::createBulletCollisionShapePack( const auto softMeshShape = static_cast(shape.get()); const auto mesh = softMeshShape->getAssimpMesh(); - pack = createBulletCollisionShapePackFromAssimpMesh(mesh); + bulletCollisionShape = createBulletCollisionShapeFromAssimpMesh(mesh); break; } @@ -409,13 +375,40 @@ BulletCollisionDetector::createBulletCollisionShapePack( << "Attempting to create unsupported shape type '" << shape->getShapeType() << "'.\n"; - bulletCollisionShape.reset(new btSphereShape(0.1)); + bulletCollisionShape = new btSphereShape(0.1); break; } } - return pack; + if (bulletCollisionShape) + return std::shared_ptr(bulletCollisionShape, deleter); + else + return std::shared_ptr(); +} + +//============================================================================== +BulletCollisionDetector:: +BulletCollisionShapeDeleter::BulletCollisionShapeDeleter( + BulletCollisionDetector* cd, const dynamics::ConstShapePtr& shape) + : mBulletCollisionDetector(cd), + mShape(shape) +{ + assert(cd); + assert(shape); +} + +//============================================================================== +void BulletCollisionDetector::BulletCollisionShapeDeleter::operator()( + btCollisionShape* bulletCollisionShape) const +{ + mBulletCollisionDetector->mShapeMap.erase(mShape); + + auto userPointer = bulletCollisionShape->getUserPointer(); + if (userPointer) + delete static_cast(userPointer); + + delete bulletCollisionShape; } @@ -484,7 +477,7 @@ void convertContacts( } //============================================================================== -BulletCollisionDetector::BulletCollsionShapePack createBulletEllipsoidMesh( +btCollisionShape* createBulletEllipsoidMesh( float sizeX, float sizeY, float sizeZ) { float v[59][3] = @@ -666,8 +659,7 @@ BulletCollisionDetector::BulletCollsionShapePack createBulletEllipsoidMesh( {56, 49, 58} }; - BulletCollisionDetector::BulletCollsionShapePack pack; - pack.triMesh.reset(new btTriangleMesh()); + auto triMesh = new btTriangleMesh(); for (auto i = 0u; i < 112; ++i) { @@ -685,24 +677,20 @@ BulletCollisionDetector::BulletCollsionShapePack createBulletEllipsoidMesh( vertices[1] = btVector3(p1[0] * sizeX, p1[1] * sizeY, p1[2] * sizeZ); vertices[2] = btVector3(p2[0] * sizeX, p2[1] * sizeY, p2[2] * sizeZ); - pack.triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); } - auto gimpactMeshShape = new btGImpactMeshShape(pack.triMesh.get()); + auto gimpactMeshShape = new btGImpactMeshShape(triMesh); gimpactMeshShape->updateBound(); - pack.collisionShape.reset(gimpactMeshShape); - - return pack; + return gimpactMeshShape; } //============================================================================== -BulletCollisionDetector::BulletCollsionShapePack -createBulletCollisionShapePackFromAssimpScene( +btCollisionShape* createBulletCollisionShapeFromAssimpScene( const Eigen::Vector3d& scale, const aiScene* scene) { - BulletCollisionDetector::BulletCollsionShapePack pack; - pack.triMesh.reset(new btTriangleMesh()); + auto triMesh = new btTriangleMesh(); for (auto i = 0u; i < scene->mNumMeshes; ++i) { @@ -717,24 +705,21 @@ createBulletCollisionShapePackFromAssimpScene( vertex.y * scale[1], vertex.z * scale[2]); } - pack.triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); } } - auto gimpactMeshShape = new btGImpactMeshShape(pack.triMesh.get()); + auto gimpactMeshShape = new btGImpactMeshShape(triMesh); gimpactMeshShape->updateBound(); + gimpactMeshShape->setUserPointer(triMesh); - pack.collisionShape.reset(gimpactMeshShape); - - return pack; + return gimpactMeshShape; } //============================================================================== -BulletCollisionDetector::BulletCollsionShapePack -createBulletCollisionShapePackFromAssimpMesh(const aiMesh* mesh) +btCollisionShape* createBulletCollisionShapeFromAssimpMesh(const aiMesh* mesh) { - BulletCollisionDetector::BulletCollsionShapePack pack; - pack.triMesh.reset(new btTriangleMesh()); + auto triMesh = new btTriangleMesh(); for (auto i = 0u; i < mesh->mNumFaces; ++i) { @@ -744,15 +729,13 @@ createBulletCollisionShapePackFromAssimpMesh(const aiMesh* mesh) const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; vertices[j] = btVector3(vertex.x, vertex.y, vertex.z); } - pack.triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); } - auto gimpactMeshShape = new btGImpactMeshShape(pack.triMesh.get()); + auto gimpactMeshShape = new btGImpactMeshShape(triMesh); gimpactMeshShape->updateBound(); - pack.collisionShape.reset(gimpactMeshShape); - - return pack; + return gimpactMeshShape; } } // anonymous namespace diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index a2f0d9c8be1d5..9d390bd7df8c0 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -53,12 +53,6 @@ class BulletCollisionDetector : public CollisionDetector friend class CollisionDetector; - struct BulletCollsionShapePack - { - std::shared_ptr collisionShape; - std::shared_ptr triMesh; - }; - static std::shared_ptr create(); /// Constructor @@ -87,29 +81,38 @@ class BulletCollisionDetector : public CollisionDetector BulletCollisionDetector() = default; // Documentation inherited - CollisionObject* createCollisionObject( + std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; - // Documentation inherited - void notifyCollisionObjectDestorying(CollisionObject* collObj) override; - - BulletCollsionShapePack claimBulletCollisionGeometry( + std::shared_ptr claimBulletCollisionGeometry( const dynamics::ConstShapePtr& shape); - // Documentation inherited - void reclaimBulletCollisionGeometry(const dynamics::ConstShapePtr& shape); +private: - BulletCollsionShapePack createBulletCollisionShapePack( - const dynamics::ConstShapePtr& shape); + /// This deleter is responsible for deleting fcl::CollisionGeometry and + /// removing it from mShapeMap when it is not shared by any CollisionObjects. + class BulletCollisionShapeDeleter final + { + public: -protected: + BulletCollisionShapeDeleter(BulletCollisionDetector* cd, + const dynamics::ConstShapePtr& shape); + + void operator()(btCollisionShape* bulletCollisionShape) const; + + private: - using ShapeMapValue = std::pair; + BulletCollisionDetector* mBulletCollisionDetector; + + dynamics::ConstShapePtr mShape; + + }; - std::map mShapeMap; + std::shared_ptr createBulletCollisionShape( + const dynamics::ConstShapePtr& shape, + const BulletCollisionShapeDeleter& deleter); - std::map mBulletCollisionObjectMap; + std::map> mShapeMap; }; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 1f71ec1104c88..424bdd3057c56 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -214,30 +214,15 @@ void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) } //============================================================================== -CollisionObject* DARTCollisionDetector::createCollisionObject( +std::unique_ptr DARTCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { - auto collObj = new DARTCollisionObject(this, shapeFrame); - warnUnsupportedShapeType(shapeFrame); - mDARTCollisionObjects.push_back(collObj); - - return collObj; + return std::unique_ptr( + new DARTCollisionObject(this, shapeFrame)); } -//============================================================================== -void DARTCollisionDetector::notifyCollisionObjectDestorying( - CollisionObject* collObj) -{ - if (!collObj) - return; - - auto casted = static_cast(collObj); - mDARTCollisionObjects.erase( - std::remove(mDARTCollisionObjects.begin(), mDARTCollisionObjects.end(), - casted), mDARTCollisionObjects.end()); -} diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index fa41b43414ad0..9dfeb66d23098 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -74,16 +74,9 @@ class DARTCollisionDetector : public CollisionDetector DARTCollisionDetector() = default; // Documentation inherited - CollisionObject* createCollisionObject( + std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; - // Documentation inherited - void notifyCollisionObjectDestorying(CollisionObject* collObj) override; - -protected: - - std::vector mDARTCollisionObjects; - }; } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 5b3f17d0cd2f5..3c6c8af808a1d 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -125,9 +125,9 @@ struct FCLCollisionCallbackData /// Collision result of DART Result* mResult; - FCLCollisionDetector::PrimitiveShape_t mPrimitiveShapeType; + FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType; - FCLCollisionDetector::ContactPointComputationMethod_t + FCLCollisionDetector::ContactPointComputationMethod mContactPointComputationMethod; /// Whether the collision iteration can stop @@ -137,9 +137,9 @@ struct FCLCollisionCallbackData FCLCollisionCallbackData( const Option& option, Result* result = nullptr, - FCLCollisionDetector::PrimitiveShape_t type + FCLCollisionDetector::PrimitiveShape type = FCLCollisionDetector::MESH, - FCLCollisionDetector::ContactPointComputationMethod_t method + FCLCollisionDetector::ContactPointComputationMethod method = FCLCollisionDetector::DART) : mOption(option), mResult(result), @@ -569,151 +569,6 @@ fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) return model; } -//============================================================================== -boost::shared_ptr createFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape, - FCLCollisionDetector::PrimitiveShape_t type) -{ - using dynamics::Shape; - using dynamics::BoxShape; - using dynamics::EllipsoidShape; - using dynamics::CylinderShape; - using dynamics::PlaneShape; - using dynamics::MeshShape; - using dynamics::SoftMeshShape; - - boost::shared_ptr fclCollGeom; - - switch (shape->getShapeType()) - { - case Shape::BOX: - { - assert(dynamic_cast(shape.get())); - - auto box = static_cast(shape.get()); - const Eigen::Vector3d& size = box->getSize(); - - if (FCLCollisionDetector::PRIMITIVE == type) - fclCollGeom.reset(new fcl::Box(size[0], size[1], size[2])); - else - fclCollGeom.reset(createCube(size[0], size[1], size[2])); - - break; - } - case Shape::ELLIPSOID: - { - assert(dynamic_cast(shape.get())); - - auto ellipsoid = static_cast(shape.get()); - const Eigen::Vector3d& size = ellipsoid->getSize(); - - if (FCLCollisionDetector::PRIMITIVE == type) - { - if (ellipsoid->isSphere()) - { - fclCollGeom.reset(new fcl::Sphere(size[0] * 0.5)); - } - else - { -#if FCL_VERSION_AT_LEAST(0,4,0) - fclCollGeom.reset( - new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5))); -#else - fclCollGeom.reset( - createEllipsoid(size[0], size[1], size[2])); -#endif - } - } - else - { - fclCollGeom.reset( - createEllipsoid(size[0], size[1], size[2])); - } - - break; - } - case Shape::CYLINDER: - { - assert(dynamic_cast(shape.get())); - - const auto cylinder = static_cast(shape.get()); - const auto radius = cylinder->getRadius(); - const auto height = cylinder->getHeight(); - - if (FCLCollisionDetector::PRIMITIVE == type) - { - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); - // TODO(JS): We still need to use mesh for cylinder because FCL 0.4.0 - // returns single contact point for cylinder yet. Once FCL support - // multiple contact points then above code will be replaced by: - // fclCollGeom.reset(new fcl::Cylinder(radius, height)); - } - else - { - fclCollGeom.reset(createCylinder( - radius, radius, height, 16, 16)); - } - - break; - } - case Shape::PLANE: - { - if (FCLCollisionDetector::PRIMITIVE == type) - { - assert(dynamic_cast(shape.get())); - auto plane = static_cast(shape.get()); - const Eigen::Vector3d normal = plane->getNormal(); - const double offset = plane->getOffset(); - fclCollGeom.reset( - new fcl::Halfspace(FCLTypes::convertVector3(normal), offset)); - } - else - { - fclCollGeom.reset(createCube(1000.0, 0.0, 1000.0)); - dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " - << "FCLCollisionDetector. We create a thin box mesh insted, where " - << "the size is [1000 0 1000].\n"; - } - - break; - } - case Shape::MESH: - { - assert(dynamic_cast(shape.get())); - - auto shapeMesh = static_cast(shape.get()); - const Eigen::Vector3d& scale = shapeMesh->getScale(); - auto aiScene = shapeMesh->getMesh(); - - fclCollGeom.reset( - createMesh(scale[0], scale[1], scale[2], aiScene)); - - break; - } - case Shape::SOFT_MESH: - { - assert(dynamic_cast(shape.get())); - - auto softMeshShape = static_cast(shape.get()); - auto aiMesh = softMeshShape->getAssimpMesh(); - - fclCollGeom.reset(createSoftMesh(aiMesh)); - - break; - } - default: - { - dterr << "[FCLCollisionDetector] Attempting to create unsupported shape " - << "type '" << shape->getShapeType() << "'.\n"; - - return nullptr; - } - } - - return fclCollGeom; -} - } // anonymous namespace @@ -728,13 +583,13 @@ std::shared_ptr FCLCollisionDetector::create() FCLCollisionDetector::~FCLCollisionDetector() { assert(mShapeMap.empty()); - assert(mFCLCollisionObjectMap.empty()); } //============================================================================== const std::string& FCLCollisionDetector::getTypeStatic() { static const std::string& type("FCL"); + return type; } @@ -820,7 +675,7 @@ bool FCLCollisionDetector::detect( //============================================================================== void FCLCollisionDetector::setPrimitiveShapeType( - FCLCollisionDetector::PrimitiveShape_t type) + FCLCollisionDetector::PrimitiveShape type) { if (type == PRIMITIVE) { @@ -836,7 +691,7 @@ void FCLCollisionDetector::setPrimitiveShapeType( } //============================================================================== -FCLCollisionDetector::PrimitiveShape_t +FCLCollisionDetector::PrimitiveShape FCLCollisionDetector::getPrimitiveShapeType() const { return mPrimitiveShapeType; @@ -844,7 +699,7 @@ FCLCollisionDetector::getPrimitiveShapeType() const //============================================================================== void FCLCollisionDetector::setContactPointComputationMethod( - FCLCollisionDetector::ContactPointComputationMethod_t method) + FCLCollisionDetector::ContactPointComputationMethod method) { if (method == FCL) { @@ -861,7 +716,7 @@ void FCLCollisionDetector::setContactPointComputationMethod( } //============================================================================== -FCLCollisionDetector::ContactPointComputationMethod_t +FCLCollisionDetector::ContactPointComputationMethod FCLCollisionDetector::getContactPointComputationMethod() const { return mContactPointComputationMethod; @@ -876,73 +731,200 @@ FCLCollisionDetector::FCLCollisionDetector() } //============================================================================== -CollisionObject* FCLCollisionDetector::createCollisionObject( +std::unique_ptr FCLCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { auto fclCollGeom = claimFCLCollisionGeometry(shapeFrame->getShape()); - auto collObj = new FCLCollisionObject(this, shapeFrame, fclCollGeom); - auto fclCollObj = collObj->getFCLCollisionObject(); - mFCLCollisionObjectMap[fclCollObj] = collObj; - - return collObj; + return std::unique_ptr( + new FCLCollisionObject(this, shapeFrame, fclCollGeom)); } //============================================================================== -void FCLCollisionDetector::notifyCollisionObjectDestorying( - CollisionObject* collObj) +boost::shared_ptr +FCLCollisionDetector::claimFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape) { - if (!collObj) - return; + const auto search = mShapeMap.find(shape); - reclaimFCLCollisionGeometry(collObj->getShape()); + if (mShapeMap.end() != search) + { + const auto& fclCollGeom = search->second; + assert(fclCollGeom.lock()); + // Ensure all the collision geometry in the map should be alive pointers. + + return fclCollGeom.lock(); + } + + auto newfclCollGeom = createFCLCollisionGeometry( + shape, mPrimitiveShapeType, FCLCollisionGeometryDeleter(this, shape)); + mShapeMap[shape] = newfclCollGeom; - auto casted = static_cast(collObj); - mFCLCollisionObjectMap.erase(casted->getFCLCollisionObject()); + return newfclCollGeom; } //============================================================================== boost::shared_ptr -FCLCollisionDetector::claimFCLCollisionGeometry( - const dynamics::ConstShapePtr& shape) +FCLCollisionDetector::createFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape, + FCLCollisionDetector::PrimitiveShape type, + const FCLCollisionGeometryDeleter& deleter) { - boost::shared_ptr fclCollGeom; + using dynamics::Shape; + using dynamics::BoxShape; + using dynamics::EllipsoidShape; + using dynamics::CylinderShape; + using dynamics::PlaneShape; + using dynamics::MeshShape; + using dynamics::SoftMeshShape; - auto search = mShapeMap.find(shape); - if (mShapeMap.end() != search) + fcl::CollisionGeometry* geom = nullptr; + + switch (shape->getShapeType()) { - auto& fclCollGeomAndCount = search->second; + case Shape::BOX: + { + assert(dynamic_cast(shape.get())); - fclCollGeom = search->second.first; + auto box = static_cast(shape.get()); + const Eigen::Vector3d& size = box->getSize(); - auto& count = fclCollGeomAndCount.second; - assert(0u != count); - count++; - } - else - { - fclCollGeom = createFCLCollisionGeometry(shape, mPrimitiveShapeType); - mShapeMap[shape] = std::make_pair(fclCollGeom, 1u); + if (FCLCollisionDetector::PRIMITIVE == type) + geom = new fcl::Box(size[0], size[1], size[2]); + else + geom = createCube(size[0], size[1], size[2]); + + break; + } + case Shape::ELLIPSOID: + { + assert(dynamic_cast(shape.get())); + + auto ellipsoid = static_cast(shape.get()); + const Eigen::Vector3d& size = ellipsoid->getSize(); + + if (FCLCollisionDetector::PRIMITIVE == type) + { + if (ellipsoid->isSphere()) + { + geom = new fcl::Sphere(size[0] * 0.5); + } + else + { +#if FCL_VERSION_AT_LEAST(0,4,0) + geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5)); +#else + geom = createEllipsoid(size[0], size[1], size[2]); +#endif + } + } + else + { + geom = createEllipsoid(size[0], size[1], size[2]); + } + + break; + } + case Shape::CYLINDER: + { + assert(dynamic_cast(shape.get())); + + const auto cylinder = static_cast(shape.get()); + const auto radius = cylinder->getRadius(); + const auto height = cylinder->getHeight(); + + if (FCLCollisionDetector::PRIMITIVE == type) + { + geom = createCylinder(radius, radius, height, 16, 16); + // TODO(JS): We still need to use mesh for cylinder because FCL 0.4.0 + // returns single contact point for cylinder yet. Once FCL support + // multiple contact points then above code will be replaced by: + // fclCollGeom.reset(new fcl::Cylinder(radius, height)); + } + else + { + geom = createCylinder(radius, radius, height, 16, 16); + } + + break; + } + case Shape::PLANE: + { + if (FCLCollisionDetector::PRIMITIVE == type) + { + assert(dynamic_cast(shape.get())); + auto plane = static_cast(shape.get()); + const Eigen::Vector3d normal = plane->getNormal(); + const double offset = plane->getOffset(); + + geom = new fcl::Halfspace(FCLTypes::convertVector3(normal), offset); + } + else + { + geom = createCube(1000.0, 0.0, 1000.0); + dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " + << "FCLCollisionDetector. We create a thin box mesh insted, where " + << "the size is [1000 0 1000].\n"; + } + + break; + } + case Shape::MESH: + { + assert(dynamic_cast(shape.get())); + + auto shapeMesh = static_cast(shape.get()); + const Eigen::Vector3d& scale = shapeMesh->getScale(); + auto aiScene = shapeMesh->getMesh(); + + geom = createMesh(scale[0], scale[1], scale[2], aiScene); + + break; + } + case Shape::SOFT_MESH: + { + assert(dynamic_cast(shape.get())); + + auto softMeshShape = static_cast(shape.get()); + auto aiMesh = softMeshShape->getAssimpMesh(); + + geom = createSoftMesh(aiMesh); + + break; + } + default: + { + dterr << "[FCLCollisionDetector] Attempting to create unsupported shape " + << "type '" << shape->getShapeType() << "'.\n"; + + return nullptr; + } } - return fclCollGeom; + if (geom) + return boost::shared_ptr(geom, deleter); + else + return boost::shared_ptr(); } //============================================================================== -void FCLCollisionDetector::reclaimFCLCollisionGeometry( +FCLCollisionDetector::FCLCollisionGeometryDeleter::FCLCollisionGeometryDeleter( + FCLCollisionDetector* cd, const dynamics::ConstShapePtr& shape) + : mFCLCollisionDetector(cd), + mShape(shape) { - auto search = mShapeMap.find(shape); - assert(mShapeMap.end() != search); - - auto& fclCollGeomAndCount = search->second; - auto& count = fclCollGeomAndCount.second; - assert(0u != count); + assert(cd); + assert(shape); +} - count--; +//============================================================================== +void FCLCollisionDetector::FCLCollisionGeometryDeleter::operator()( + fcl::CollisionGeometry* geom) const +{ + mFCLCollisionDetector->mShapeMap.erase(mShape); - if (0u == count) - mShapeMap.erase(search); + delete geom; } diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 3a401a4a6ff5a..6eba07e9fc89e 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -40,6 +40,7 @@ #include #include +#include // This should be removed once we migrate to fcl 0.5 #include "dart/collision/CollisionDetector.h" namespace dart { @@ -53,6 +54,27 @@ class FCLCollisionDetector : public CollisionDetector static std::shared_ptr create(); + /// FCL's primitive shape support is still not complete. FCL 0.4.0 fixed lots + /// of the bugs, but it still returns single contact point per shape pair + /// except for box-box collision. For this reason, we recommend using mesh for + /// primitive shapes until FCL fully support primitive shapes. + enum PrimitiveShape + { + MESH = 0, + PRIMITIVE + }; + // TODO(JS): Update comment + + /// FCL's contact computation is still (at least until 0.4.0) not correct for + /// mesh collision. We recommend using DART's contact point computation + /// instead until it's fixed in FCL. + enum ContactPointComputationMethod + { + DART = 0, + FCL + }; + // TODO(JS): Update comment + /// Constructor virtual ~FCLCollisionDetector(); @@ -73,36 +95,17 @@ class FCLCollisionDetector : public CollisionDetector bool detect(CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) override; - /// FCL's primitive shape support is still not complete. FCL 0.4.0 fixed lots - /// of the bugs, but it still returns single contact point per shape pair - /// except for box-box collision. For this reason, we recommend using mesh for - /// primitive shapes until FCL fully support primitive shapes. - enum PrimitiveShape_t - { - MESH = 0, - PRIMITIVE - }; - /// Set primitive shape type - void setPrimitiveShapeType(PrimitiveShape_t type); + void setPrimitiveShapeType(PrimitiveShape type); /// Get primitive shape type - PrimitiveShape_t getPrimitiveShapeType() const; - - /// FCL's contact computation is still (at least until 0.4.0) not correct for - /// mesh collision. We recommend using DART's contact point computation - /// instead until it's fixed in FCL. - enum ContactPointComputationMethod_t - { - DART = 0, - FCL - }; + PrimitiveShape getPrimitiveShapeType() const; /// Set contact point computation method - void setContactPointComputationMethod(ContactPointComputationMethod_t method); + void setContactPointComputationMethod(ContactPointComputationMethod method); /// Get contact point computation method - ContactPointComputationMethod_t getContactPointComputationMethod() const; + ContactPointComputationMethod getContactPointComputationMethod() const; protected: @@ -110,31 +113,55 @@ class FCLCollisionDetector : public CollisionDetector FCLCollisionDetector(); // Documentation inherited - CollisionObject* createCollisionObject( + std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; - // Documentation inherited - void notifyCollisionObjectDestorying(CollisionObject* collObj) override; - - /// + /// Return fcl::CollisionGeometry associated with give Shape. New + /// fcl::CollisionGeome will be created if it hasn't created yet. boost::shared_ptr claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape); - /// - void reclaimFCLCollisionGeometry(const dynamics::ConstShapePtr& shape); - protected: - using ShapeMapValue - = std::pair, size_t>; + PrimitiveShape mPrimitiveShapeType; + + ContactPointComputationMethod mContactPointComputationMethod; + +private: + + /// This deleter is responsible for deleting fcl::CollisionGeometry and + /// removing it from mShapeMap when it is not shared by any CollisionObjects. + class FCLCollisionGeometryDeleter final + { + public: + + FCLCollisionGeometryDeleter(FCLCollisionDetector* cd, + const dynamics::ConstShapePtr& shape); - std::map mShapeMap; + void operator()(fcl::CollisionGeometry* geom) const; + + private: + + FCLCollisionDetector* mFCLCollisionDetector; + + dynamics::ConstShapePtr mShape; + + }; - std::map mFCLCollisionObjectMap; + /// Create fcl::CollisionGeometry with the custom deleter + /// FCLCollisionGeometryDeleter + boost::shared_ptr createFCLCollisionGeometry( + const dynamics::ConstShapePtr& shape, + FCLCollisionDetector::PrimitiveShape type, + const FCLCollisionGeometryDeleter& deleter); - PrimitiveShape_t mPrimitiveShapeType; + using ShapeMap = std::map>; + // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate + // to 0.5 or greater, this also should be changed to + // std::weak_ptr - ContactPointComputationMethod_t mContactPointComputationMethod; + ShapeMap mShapeMap; }; diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 3c3800298916d..22ce66ccb6e63 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -585,8 +585,8 @@ TEST_F(COLLISION, SimpleFrames) // testSimpleFrames(fcl_mesh_fcl); #if HAVE_BULLET_COLLISION -// auto bullet = BulletCollisionDetector::create(); -// testSimpleFrames(bullet); + auto bullet = BulletCollisionDetector::create(); + testSimpleFrames(bullet); #endif auto dart = DARTCollisionDetector::create(); From 202ab75604c945ed5bbfb2395e768d191f34248f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 4 Apr 2016 17:02:24 -0400 Subject: [PATCH 52/67] Fix btCollisionShape ownership management --- dart/collision/CollisionDetector.cpp | 108 ++++++++++++++++-- dart/collision/CollisionDetector.h | 79 ++++++++++++- .../bullet/BulletCollisionDetector.cpp | 100 +++++++++------- .../bullet/BulletCollisionDetector.h | 35 ++---- dart/collision/dart/DARTCollisionDetector.cpp | 7 ++ dart/collision/dart/DARTCollisionDetector.h | 2 +- dart/collision/fcl/FCLCollisionDetector.cpp | 5 +- 7 files changed, 253 insertions(+), 83 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 9222e898ed828..ca48bc6069050 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -57,7 +57,6 @@ CollisionDetector::createCollisionGroupAsSharedPtr() //============================================================================== CollisionDetector::CollisionDetector() - : mCollisionObjectDeleter(this) { // Do nothing } @@ -65,12 +64,97 @@ CollisionDetector::CollisionDetector() //============================================================================== CollisionDetector::~CollisionDetector() { - assert(mCollisionObjectMap.empty()); + // Do nothing } //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) +{ + if (!mCollisionObjectManager) + mCollisionObjectManager.reset(new NoneSharingCollisionObjectManager(this)); + + return mCollisionObjectManager->claimCollisionObject(shapeFrame); +} + +//============================================================================== +void CollisionDetector::notifyCollisionObjectDestorying( + CollisionObject* /*object*/) +{ + // Do nothing +} + +//============================================================================== +CollisionDetector::CollisionObjectManager::CollisionObjectManager( + CollisionDetector* cd) + : mCollisionDetector(cd) +{ + assert(cd); +} + +//============================================================================== +CollisionDetector:: +NoneSharingCollisionObjectManager::NoneSharingCollisionObjectManager( + CollisionDetector* cd) + : CollisionDetector::CollisionObjectManager(cd), + mCollisionObjectDeleter(this) +{ + // Do nothing +} + +//============================================================================== +std::shared_ptr +CollisionDetector::NoneSharingCollisionObjectManager::claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) +{ + auto uniqueObject = mCollisionDetector->createCollisionObject(shapeFrame); + auto sharedObject = std::shared_ptr( + uniqueObject.release(), mCollisionObjectDeleter); + + return sharedObject; +} + +//============================================================================== +CollisionDetector::NoneSharingCollisionObjectManager:: +CollisionObjectDeleter::CollisionObjectDeleter( + NoneSharingCollisionObjectManager* mgr) + : mCollisionObjectManager(mgr) +{ + assert(mgr); +} + +//============================================================================== +void +CollisionDetector::NoneSharingCollisionObjectManager +::CollisionObjectDeleter::operator()(CollisionObject* object) const +{ + mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestorying( + object); + + delete object; +} + +//============================================================================== +CollisionDetector:: +SharingCollisionObjectManager::SharingCollisionObjectManager( + CollisionDetector* cd) + : CollisionDetector::CollisionObjectManager(cd), + mCollisionObjectDeleter(this) +{ + // Do nothing +} + +//============================================================================== +CollisionDetector:: +SharingCollisionObjectManager::~SharingCollisionObjectManager() +{ + assert(mCollisionObjectMap.empty()); +} + +//============================================================================== +std::shared_ptr +CollisionDetector::SharingCollisionObjectManager::claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) { const auto search = mCollisionObjectMap.find(shapeFrame); @@ -83,7 +167,7 @@ std::shared_ptr CollisionDetector::claimCollisionObject( return collObj.lock(); } - auto uniqueObject = createCollisionObject(shapeFrame); + auto uniqueObject = mCollisionDetector->createCollisionObject(shapeFrame); auto sharedObject = std::shared_ptr( uniqueObject.release(), mCollisionObjectDeleter); @@ -93,18 +177,22 @@ std::shared_ptr CollisionDetector::claimCollisionObject( } //============================================================================== -CollisionDetector::CollisionObjectDeleter::CollisionObjectDeleter( - CollisionDetector* cd) - : mCollisionDetector(cd) +CollisionDetector::SharingCollisionObjectManager:: +CollisionObjectDeleter::CollisionObjectDeleter( + SharingCollisionObjectManager* mgr) + : mCollisionObjectManager(mgr) { - // Do nothing + assert(mgr); } //============================================================================== -void CollisionDetector::CollisionObjectDeleter::operator()( - CollisionObject* object) const +void +CollisionDetector::SharingCollisionObjectManager +::CollisionObjectDeleter::operator()(CollisionObject* object) const { - mCollisionDetector->mCollisionObjectMap.erase(object->getShapeFrame()); + mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestorying( + object); + mCollisionObjectManager->mCollisionObjectMap.erase(object->getShapeFrame()); delete object; } diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 744110b43d5d4..f06de8db7e8d9 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -104,6 +104,10 @@ class CollisionDetector : public std::enable_shared_from_this protected: + class CollisionObjectManager; + class NoneSharingCollisionObjectManager; + class SharingCollisionObjectManager; + /// Constructor CollisionDetector(); @@ -116,15 +120,84 @@ class CollisionDetector : public std::enable_shared_from_this virtual std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) = 0; + /// Notify that a CollisionObject is destroying. Do nothing by default. + virtual void notifyCollisionObjectDestorying(CollisionObject* object); + + std::unique_ptr mCollisionObjectManager; + +}; + +class CollisionDetector::CollisionObjectManager +{ +public: + + /// Constructor + CollisionObjectManager(CollisionDetector* cd); + + /// Claim CollisionObject associated with shapeFrame. New CollisionObject + /// will be created if it hasn't created yet for shapeFrame. + virtual std::shared_ptr claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame) = 0; + +protected: + + CollisionDetector* mCollisionDetector; + +}; + +class CollisionDetector::NoneSharingCollisionObjectManager final : + public CollisionDetector::CollisionObjectManager +{ +public: + + /// Constructor + NoneSharingCollisionObjectManager(CollisionDetector* cd); + + // Documentation inherited + std::shared_ptr claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame); + private: -\ + + /// This deleter is responsible for deleting CollisionObject and removing it + /// from mCollisionObjectMap when it is not shared by any CollisionGroups. + struct CollisionObjectDeleter final + { + NoneSharingCollisionObjectManager* mCollisionObjectManager; + + CollisionObjectDeleter(NoneSharingCollisionObjectManager* mgr); + + void operator()(CollisionObject* object) const; + }; + + const CollisionObjectDeleter mCollisionObjectDeleter; + +}; + +class CollisionDetector::SharingCollisionObjectManager final : + public CollisionDetector::CollisionObjectManager +{ +public: + + /// Constructor + SharingCollisionObjectManager(CollisionDetector* cd); + + /// Destructor + virtual ~SharingCollisionObjectManager(); + + // Documentation inherited + std::shared_ptr claimCollisionObject( + const dynamics::ShapeFrame* shapeFrame); + +private: + /// This deleter is responsible for deleting CollisionObject and removing it /// from mCollisionObjectMap when it is not shared by any CollisionGroups. struct CollisionObjectDeleter final { - CollisionDetector* mCollisionDetector; + SharingCollisionObjectManager* mCollisionObjectManager; - CollisionObjectDeleter(CollisionDetector* cd); + CollisionObjectDeleter(SharingCollisionObjectManager* mgr); void operator()(CollisionObject* object) const; }; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index c98ffc913dd95..6f1b92ab67c65 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -238,44 +238,85 @@ bool BulletCollisionDetector::detect( return result.isCollision(); } +//============================================================================== +BulletCollisionDetector::BulletCollisionDetector() + : CollisionDetector() +{ + mCollisionObjectManager.reset(new NoneSharingCollisionObjectManager(this)); +} + //============================================================================== std::unique_ptr BulletCollisionDetector::createCollisionObject( const dynamics::ShapeFrame* shapeFrame) { - auto bulletCollShape = claimBulletCollisionGeometry(shapeFrame->getShape()); + auto bulletCollShape = claimBulletCollisionShape(shapeFrame->getShape()); return std::unique_ptr( - new BulletCollisionObject(this, shapeFrame, bulletCollShape.get())); + new BulletCollisionObject(this, shapeFrame, bulletCollShape)); } //============================================================================== -std::shared_ptr -BulletCollisionDetector::claimBulletCollisionGeometry( +void BulletCollisionDetector::notifyCollisionObjectDestorying( + CollisionObject* object) +{ + reclaimBulletCollisionShape(object->getShape()); +} + +//============================================================================== +btCollisionShape* BulletCollisionDetector::claimBulletCollisionShape( const dynamics::ConstShapePtr& shape) { const auto search = mShapeMap.find(shape); if (mShapeMap.end() != search) { - const auto& bulletCollShape = search->second; - assert(bulletCollShape.lock()); - // Ensure all the collision shape in the map should be alive pointers. + auto& bulletCollShapeAndCount = search->second; + + auto& bulletCollShape = bulletCollShapeAndCount.first; + auto& count = bulletCollShapeAndCount.second; + assert(0u != count); - return bulletCollShape.lock(); + count++; + + return bulletCollShape; } - auto newBulletCollisionShape = createBulletCollisionShape( - shape, BulletCollisionShapeDeleter(this, shape)); - mShapeMap[shape] = newBulletCollisionShape; + auto newBulletCollisionShape = createBulletCollisionShape(shape); + mShapeMap[shape] = std::make_pair(newBulletCollisionShape, 1u); return newBulletCollisionShape; } //============================================================================== -std::shared_ptr -BulletCollisionDetector::createBulletCollisionShape( - const dynamics::ConstShapePtr& shape, - const BulletCollisionShapeDeleter& deleter) +void BulletCollisionDetector::reclaimBulletCollisionShape( + const dynamics::ConstShapePtr& shape) +{ + auto search = mShapeMap.find(shape); + + assert(mShapeMap.end() != search); + + auto& bulletCollShapeAndCount = search->second; + + auto& bulletCollShape = bulletCollShapeAndCount.first; + auto& count = bulletCollShapeAndCount.second; + + count--; + + if (0u == count) + { + auto userPointer = bulletCollShape->getUserPointer(); + if (userPointer) + delete static_cast(userPointer); + + delete bulletCollShape; + + mShapeMap.erase(search); + } +} + +//============================================================================== +btCollisionShape* BulletCollisionDetector::createBulletCollisionShape( + const dynamics::ConstShapePtr& shape) { using dynamics::Shape; using dynamics::BoxShape; @@ -381,34 +422,7 @@ BulletCollisionDetector::createBulletCollisionShape( } } - if (bulletCollisionShape) - return std::shared_ptr(bulletCollisionShape, deleter); - else - return std::shared_ptr(); -} - -//============================================================================== -BulletCollisionDetector:: -BulletCollisionShapeDeleter::BulletCollisionShapeDeleter( - BulletCollisionDetector* cd, const dynamics::ConstShapePtr& shape) - : mBulletCollisionDetector(cd), - mShape(shape) -{ - assert(cd); - assert(shape); -} - -//============================================================================== -void BulletCollisionDetector::BulletCollisionShapeDeleter::operator()( - btCollisionShape* bulletCollisionShape) const -{ - mBulletCollisionDetector->mShapeMap.erase(mShape); - - auto userPointer = bulletCollisionShape->getUserPointer(); - if (userPointer) - delete static_cast(userPointer); - - delete bulletCollisionShape; + return bulletCollisionShape; } diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 9d390bd7df8c0..031677122186c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -78,41 +78,28 @@ class BulletCollisionDetector : public CollisionDetector protected: /// Constructor - BulletCollisionDetector() = default; + BulletCollisionDetector(); // Documentation inherited std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; - std::shared_ptr claimBulletCollisionGeometry( - const dynamics::ConstShapePtr& shape); + // Documentation inherited + void notifyCollisionObjectDestorying(CollisionObject* object) override; private: - /// This deleter is responsible for deleting fcl::CollisionGeometry and - /// removing it from mShapeMap when it is not shared by any CollisionObjects. - class BulletCollisionShapeDeleter final - { - public: - - BulletCollisionShapeDeleter(BulletCollisionDetector* cd, - const dynamics::ConstShapePtr& shape); - - void operator()(btCollisionShape* bulletCollisionShape) const; - - private: - - BulletCollisionDetector* mBulletCollisionDetector; - - dynamics::ConstShapePtr mShape; + btCollisionShape* claimBulletCollisionShape( + const dynamics::ConstShapePtr& shape); - }; + void reclaimBulletCollisionShape( + const dynamics::ConstShapePtr& shape); - std::shared_ptr createBulletCollisionShape( - const dynamics::ConstShapePtr& shape, - const BulletCollisionShapeDeleter& deleter); + btCollisionShape* createBulletCollisionShape( + const dynamics::ConstShapePtr& shape); - std::map> mShapeMap; + std::map> mShapeMap; }; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 424bdd3057c56..9d485184c88cc 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -186,6 +186,13 @@ bool DARTCollisionDetector::detect( return result.isCollision(); } +//============================================================================== +DARTCollisionDetector::DARTCollisionDetector() + : CollisionDetector() +{ + mCollisionObjectManager.reset(new SharingCollisionObjectManager(this)); +} + //============================================================================== void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) { diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 9dfeb66d23098..17d1036cab4af 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -71,7 +71,7 @@ class DARTCollisionDetector : public CollisionDetector protected: /// Constructor - DARTCollisionDetector() = default; + DARTCollisionDetector(); // Documentation inherited std::unique_ptr createCollisionObject( diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 3c6c8af808a1d..4c5804fc23ef5 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -724,10 +724,11 @@ FCLCollisionDetector::getContactPointComputationMethod() const //============================================================================== FCLCollisionDetector::FCLCollisionDetector() - : mPrimitiveShapeType(MESH), + : CollisionDetector(), + mPrimitiveShapeType(MESH), mContactPointComputationMethod(DART) { - // Do nothing + mCollisionObjectManager.reset(new SharingCollisionObjectManager(this)); } //============================================================================== From 1230a2d82db670ce38ef7d007b660919118912c1 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 4 Apr 2016 17:25:26 -0400 Subject: [PATCH 53/67] Update comment on FCLCollisionDetector --- dart/collision/CollisionDetector.h | 2 + .../bullet/BulletCollisionDetector.h | 2 + dart/collision/fcl/FCLCollisionDetector.h | 38 ++++++++++++------- 3 files changed, 29 insertions(+), 13 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index f06de8db7e8d9..a8096a585e2e6 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -123,6 +123,8 @@ class CollisionDetector : public std::enable_shared_from_this /// Notify that a CollisionObject is destroying. Do nothing by default. virtual void notifyCollisionObjectDestorying(CollisionObject* object); +protected: + std::unique_ptr mCollisionObjectManager; }; diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 031677122186c..04793520a7ffa 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -98,6 +98,8 @@ class BulletCollisionDetector : public CollisionDetector btCollisionShape* createBulletCollisionShape( const dynamics::ConstShapePtr& shape); +private: + std::map> mShapeMap; diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 6eba07e9fc89e..7b3ae72655a27 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -54,26 +54,36 @@ class FCLCollisionDetector : public CollisionDetector static std::shared_ptr create(); - /// FCL's primitive shape support is still not complete. FCL 0.4.0 fixed lots - /// of the bugs, but it still returns single contact point per shape pair - /// except for box-box collision. For this reason, we recommend using mesh for - /// primitive shapes until FCL fully support primitive shapes. + /// Whether to use analytic collision checking for primitive shapes. + /// + /// PRIMITIVE: Use FCL's analytic collision checking for primitive shapes. + /// MESH: Don't use it. Instead, use approximate mesh shapes for the primitive + /// shapes. The contact result is probably less accurate than the analytic + /// result. + /// + /// Warning: FCL's primitive shape support is not complete. FCL 0.4.0 improved + /// the support alot, but it still returns single contact point for a shape + /// pair except for box-box collision. For this reason, we recommend using + /// MESH until FCL fully supports primitive shapes. enum PrimitiveShape { - MESH = 0, - PRIMITIVE + PRIMITIVE = 0, + MESH }; - // TODO(JS): Update comment - /// FCL's contact computation is still (at least until 0.4.0) not correct for - /// mesh collision. We recommend using DART's contact point computation - /// instead until it's fixed in FCL. + /// Whether to use FCL's contact point computation. + /// + /// FCL: Use FCL's contact point computation. + /// DART: Use DART's own contact point computation + /// + /// Warning: FCL's contact computation is not correct. See: + /// https://github.com/flexible-collision-library/fcl/issues/106 + /// We recommend using DART until it's fixed in FCL. enum ContactPointComputationMethod { - DART = 0, - FCL + FCL = 0, + DART }; - // TODO(JS): Update comment /// Constructor virtual ~FCLCollisionDetector(); @@ -155,6 +165,8 @@ class FCLCollisionDetector : public CollisionDetector FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); +private: + using ShapeMap = std::map>; // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate From 517203503ff37c177409b23f72ce7fb8f6d82787 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 5 Apr 2016 12:29:30 -0400 Subject: [PATCH 54/67] Update in response to @mkoval 's comments on #631 --- dart/collision/CollisionDetector.cpp | 12 ------- dart/collision/CollisionDetector.h | 4 +-- dart/collision/CollisionFilter.cpp | 2 +- dart/collision/CollisionGroup.cpp | 24 +++++--------- dart/collision/CollisionGroup.h | 32 ++++++++----------- dart/collision/CollisionObject.cpp | 6 ---- dart/collision/CollisionObject.h | 2 +- .../bullet/BulletCollisionDetector.cpp | 25 ++++++--------- .../collision/bullet/BulletCollisionGroup.cpp | 16 +++------- dart/collision/bullet/BulletCollisionGroup.h | 10 +++--- dart/collision/dart/DARTCollisionDetector.cpp | 26 +++++++++------ dart/collision/dart/DARTCollisionGroup.cpp | 14 +++----- dart/collision/dart/DARTCollisionGroup.h | 10 +++--- dart/collision/fcl/FCLCollisionDetector.cpp | 27 +++++++--------- dart/collision/fcl/FCLCollisionGroup.cpp | 14 +++----- dart/collision/fcl/FCLCollisionGroup.h | 10 +++--- 16 files changed, 92 insertions(+), 142 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index ca48bc6069050..4a64c0b5f06a5 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -55,18 +55,6 @@ CollisionDetector::createCollisionGroupAsSharedPtr() return std::shared_ptr(createCollisionGroup().release()); } -//============================================================================== -CollisionDetector::CollisionDetector() -{ - // Do nothing -} - -//============================================================================== -CollisionDetector::~CollisionDetector() -{ - // Do nothing -} - //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index a8096a585e2e6..a024ce6c38452 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -62,7 +62,7 @@ class CollisionDetector : public std::enable_shared_from_this friend class CollisionGroup; /// Destructor - virtual ~CollisionDetector(); + virtual ~CollisionDetector() = default; /// Return collision detection engine type in std::string virtual const std::string& getType() const = 0; @@ -109,7 +109,7 @@ class CollisionDetector : public std::enable_shared_from_this class SharingCollisionObjectManager; /// Constructor - CollisionDetector(); + CollisionDetector() = default; /// Claim CollisionObject associated with shapeFrame. New CollisionObject /// will be created if it hasn't created yet for shapeFrame. diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp index 90d026a7cd80e..ccff47cfa8baf 100644 --- a/dart/collision/CollisionFilter.cpp +++ b/dart/collision/CollisionFilter.cpp @@ -55,7 +55,7 @@ bool BodyNodeCollisionFilter::needCollision( if (!shapeNode1 || !shapeNode2) return true; - // Assume non-ShapeNode always collides with others. + // We assume that non-ShapeNode is always being checked collision. auto bodyNode1 = shapeNode1->getBodyNodePtr(); auto bodyNode2 = shapeNode2->getBodyNodePtr(); diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 1e4ac7e4e4eaf..5bf611dae9e6c 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -54,21 +54,15 @@ CollisionGroup::CollisionGroup(const CollisionDetectorPtr& collisionDetector) } //============================================================================== -CollisionGroup::~CollisionGroup() +CollisionDetectorPtr CollisionGroup::getCollisionDetector() { - // Do nothing -} - -//============================================================================== -CollisionDetector* CollisionGroup::getCollisionDetector() -{ - return mCollisionDetector.get(); + return mCollisionDetector; } //============================================================================== -const CollisionDetector* CollisionGroup::getCollisionDetector() const +ConstCollisionDetectorPtr CollisionGroup::getCollisionDetector() const { - return mCollisionDetector.get(); + return mCollisionDetector; } //============================================================================== @@ -82,7 +76,7 @@ void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame); - notifyCollisionObjectAdded(collObj.get()); + addCollisionObjectToEngine(collObj.get()); mShapeFrames.push_back(shapeFrame); mCollisionObjects.push_back(collObj); @@ -109,16 +103,14 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) return; const auto search - = std::find_if(mShapeFrames.begin(), mShapeFrames.end(), - [&](const dynamics::ShapeFrame* it) - { return it == shapeFrame; }); + = std::find(mShapeFrames.begin(), mShapeFrames.end(), shapeFrame); if (mShapeFrames.end() == search) return; const size_t index = search - mShapeFrames.begin(); - notifyCollisionObjectRemoved(mCollisionObjects[index].get()); + removeCollisionObjectFromEngine(mCollisionObjects[index].get()); mShapeFrames.erase(search); mCollisionObjects.erase(mCollisionObjects.begin() + index); @@ -141,7 +133,7 @@ void CollisionGroup::removeShapeFramesOf() //============================================================================== void CollisionGroup::removeAllShapeFrames() { - notifyAllCollisionObjectsRemoved(); + removeAllCollisionObjectsFromEngine(); mShapeFrames.clear(); mCollisionObjects.clear(); diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 350154fd1210b..d9d578fc1074a 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -58,14 +58,14 @@ class CollisionGroup // CollisionGroup also can be created from CollisionDetector::create() /// Destructor - virtual ~CollisionGroup(); + virtual ~CollisionGroup() = default; /// Return collision detection engine associated with this CollisionGroup - CollisionDetector* getCollisionDetector(); + CollisionDetectorPtr getCollisionDetector(); /// Return (const) collision detection engine associated with this /// CollisionGroup - const CollisionDetector* getCollisionDetector() const; + ConstCollisionDetectorPtr getCollisionDetector() const; /// Add a ShapeFrame to this CollisionGroup void addShapeFrame(const dynamics::ShapeFrame* shapeFrame); @@ -170,11 +170,11 @@ class CollisionGroup /// Return number of ShapeFrames added to this CollisionGroup size_t getNumShapeFrames() const; - /// Return index-th ShapeFrames registered to this CollisionGroup. Return - /// nullptr if the index is out of the range. + /// Return index-th ShapeFrames added to this CollisionGroup. Return nullptr + /// if the index is out of the range. const dynamics::ShapeFrame* getShapeFrame(size_t index) const; - /// Return all the ShapeFrames registered to this CollisionGroup + /// Return all the ShapeFrames added to this CollisionGroup const std::vector& getShapeFrames() const; /// Perform collision detection within this CollisionGroup. @@ -200,22 +200,18 @@ class CollisionGroup /// added to or removed from this CollisionGroup. virtual void initializeEngineData() = 0; - /// Notify that a CollisionObject is added so that the collision detection - /// engine do some relevant work - virtual void notifyCollisionObjectAdded(CollisionObject* object) = 0; + /// Add CollisionObject to the collision detection engine + virtual void addCollisionObjectToEngine(CollisionObject* object) = 0; - /// Notify that CollisionObjects are added so that the collision detection - /// engine do some relevant work - virtual void notifyCollisionObjectsAdded( + /// Add CollisionObjects to the collision detection engine + virtual void addCollisionObjectsToEngine( const std::vector& collObjects) = 0; - /// Notify that a CollisionObject is removed so that the collision detection - /// engine do some relevant work - virtual void notifyCollisionObjectRemoved(CollisionObject* object) = 0; + /// Remove CollisionObject from the collision detection engine + virtual void removeCollisionObjectFromEngine(CollisionObject* object) = 0; - /// Notify that all the CollisionObjects are remove so that the collision - /// detection engine do some relevant work - virtual void notifyAllCollisionObjectsRemoved() = 0; + /// Remove all the CollisionObjects from the collision detection engine + virtual void removeAllCollisionObjectsFromEngine() = 0; /// Update the collision detection engine data such as broadphase algorithm. /// This function will be called ahead of every collision checking. diff --git a/dart/collision/CollisionObject.cpp b/dart/collision/CollisionObject.cpp index 624be8f29af86..15292b54c7519 100644 --- a/dart/collision/CollisionObject.cpp +++ b/dart/collision/CollisionObject.cpp @@ -42,12 +42,6 @@ namespace dart { namespace collision { -//============================================================================== -CollisionObject::~CollisionObject() -{ - // Do nothing -} - //============================================================================== CollisionDetector* CollisionObject::getCollisionDetector() { diff --git a/dart/collision/CollisionObject.h b/dart/collision/CollisionObject.h index cf9c8cc05594a..342a9d451760d 100644 --- a/dart/collision/CollisionObject.h +++ b/dart/collision/CollisionObject.h @@ -52,7 +52,7 @@ class CollisionObject friend class CollisionGroup; /// Destructor - virtual ~CollisionObject(); + virtual ~CollisionObject() = default; /// Return collision detection engine associated with this CollisionObject CollisionDetector* getCollisionDetector(); diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 6f1b92ab67c65..8b25d673e24a1 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -176,12 +176,12 @@ bool BulletCollisionDetector::detect( { result.clear(); - if (!group) - return false; - - if (group->getCollisionDetector()->getType() - != BulletCollisionDetector::getTypeStatic()) + if (this != group->getCollisionDetector().get()) { + dterr << "[BulletCollisionDetector::detect] Attempting to check collision " + << "for a collision group that is created from a different collision " + << "detector instance.\n"; + return false; } @@ -207,18 +207,13 @@ bool BulletCollisionDetector::detect( { result.clear(); - if (!group1 || !group2) - return false; - - if (group1->getCollisionDetector()->getType() - != BulletCollisionDetector::getTypeStatic()) + if ((this != group1->getCollisionDetector().get()) + || (this != group2->getCollisionDetector().get())) { - return false; - } + dterr << "[BulletCollisionDetector::detect] Attempting to check collision " + << "for a collision group that is created from a different collision " + << "detector instance.\n"; - if (group2->getCollisionDetector()->getType() - != BulletCollisionDetector::getTypeStatic()) - { return false; } diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index bb4c9a19d154d..916b2254e9768 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -58,12 +58,6 @@ BulletCollisionGroup::BulletCollisionGroup( // Do nothing } -//============================================================================== -BulletCollisionGroup::~BulletCollisionGroup() -{ - // Do nothing -} - //============================================================================== void BulletCollisionGroup::initializeEngineData() { @@ -71,7 +65,7 @@ void BulletCollisionGroup::initializeEngineData() } //============================================================================== -void BulletCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) +void BulletCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) { auto casted = static_cast(object); @@ -81,7 +75,7 @@ void BulletCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) } //============================================================================== -void BulletCollisionGroup::notifyCollisionObjectsAdded( +void BulletCollisionGroup::addCollisionObjectsToEngine( const std::vector& collObjects) { for (auto collObj : collObjects) @@ -96,7 +90,7 @@ void BulletCollisionGroup::notifyCollisionObjectsAdded( } //============================================================================== -void BulletCollisionGroup::notifyCollisionObjectRemoved( +void BulletCollisionGroup::removeCollisionObjectFromEngine( CollisionObject* object) { auto casted = static_cast(object); @@ -108,10 +102,10 @@ void BulletCollisionGroup::notifyCollisionObjectRemoved( } //============================================================================== -void BulletCollisionGroup::notifyAllCollisionObjectsRemoved() +void BulletCollisionGroup::removeAllCollisionObjectsFromEngine() { for (const auto& collisionObject : getCollisionObjects()) - notifyCollisionObjectRemoved(collisionObject.get()); + removeCollisionObjectFromEngine(collisionObject.get()); initializeEngineData(); } diff --git a/dart/collision/bullet/BulletCollisionGroup.h b/dart/collision/bullet/BulletCollisionGroup.h index f6f09b3af4f84..978be6f059e07 100644 --- a/dart/collision/bullet/BulletCollisionGroup.h +++ b/dart/collision/bullet/BulletCollisionGroup.h @@ -54,7 +54,7 @@ class BulletCollisionGroup : public CollisionGroup BulletCollisionGroup(const CollisionDetectorPtr& collisionDetector); /// Destructor - virtual ~BulletCollisionGroup(); + virtual ~BulletCollisionGroup() = default; protected: @@ -62,17 +62,17 @@ class BulletCollisionGroup : public CollisionGroup void initializeEngineData() override; // Documentation inherited - void notifyCollisionObjectAdded(CollisionObject* object) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void notifyCollisionObjectsAdded( + void addCollisionObjectsToEngine( const std::vector& collObjects) override; // Documentation inherited - void notifyCollisionObjectRemoved(CollisionObject* object) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void notifyAllCollisionObjectsRemoved() override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited void updateCollisionGroupEngineData() override; diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 9d485184c88cc..61b64ec1861b1 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -95,9 +95,14 @@ bool DARTCollisionDetector::detect( { result.clear(); - assert(group); - assert(group->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); + if (this != group->getCollisionDetector().get()) + { + dterr << "[DARTCollisionDetector::detect] Attempting to check collision " + << "for a collision group that is created from a different collision " + << "detector instance.\n"; + + return false; + } auto objects = group->getCollisionObjects(); @@ -143,12 +148,15 @@ bool DARTCollisionDetector::detect( { result.clear(); - assert(group1); - assert(group2); - assert(group1->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); - assert(group2->getCollisionDetector()->getType() - == DARTCollisionDetector::getTypeStatic()); + if ((this != group1->getCollisionDetector().get()) + || (this != group2->getCollisionDetector().get())) + { + dterr << "[DARTCollisionDetector::detect] Attempting to check collision " + << "for a collision group that is created from a different collision " + << "detector instance.\n"; + + return false; + } auto objects1 = group1->getCollisionObjects(); auto objects2 = group2->getCollisionObjects(); diff --git a/dart/collision/dart/DARTCollisionGroup.cpp b/dart/collision/dart/DARTCollisionGroup.cpp index 944ceaf292770..2c270d6e57526 100644 --- a/dart/collision/dart/DARTCollisionGroup.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -49,12 +49,6 @@ DARTCollisionGroup::DARTCollisionGroup( // Do nothing } -//============================================================================== -DARTCollisionGroup::~DARTCollisionGroup() -{ - // Do nothing -} - //============================================================================== void DARTCollisionGroup::initializeEngineData() { @@ -62,27 +56,27 @@ void DARTCollisionGroup::initializeEngineData() } //============================================================================== -void DARTCollisionGroup::notifyCollisionObjectAdded(CollisionObject* /*object*/) +void DARTCollisionGroup::addCollisionObjectToEngine(CollisionObject* /*object*/) { // Do nothing } //============================================================================== -void DARTCollisionGroup::notifyCollisionObjectsAdded( +void DARTCollisionGroup::addCollisionObjectsToEngine( const std::vector& /*collObjects*/) { // Do nothing } //============================================================================== -void DARTCollisionGroup::notifyCollisionObjectRemoved( +void DARTCollisionGroup::removeCollisionObjectFromEngine( CollisionObject* /*object*/) { // Do nothing } //============================================================================== -void DARTCollisionGroup::notifyAllCollisionObjectsRemoved() +void DARTCollisionGroup::removeAllCollisionObjectsFromEngine() { // Do nothing } diff --git a/dart/collision/dart/DARTCollisionGroup.h b/dart/collision/dart/DARTCollisionGroup.h index d50f44d7d7e8e..ae33c7d8c37f6 100644 --- a/dart/collision/dart/DARTCollisionGroup.h +++ b/dart/collision/dart/DARTCollisionGroup.h @@ -52,7 +52,7 @@ class DARTCollisionGroup : public CollisionGroup DARTCollisionGroup(const CollisionDetectorPtr& collisionDetector); /// Destructor - virtual ~DARTCollisionGroup(); + virtual ~DARTCollisionGroup() = default; protected: @@ -60,17 +60,17 @@ class DARTCollisionGroup : public CollisionGroup void initializeEngineData() override; // Documentation inherited - void notifyCollisionObjectAdded(CollisionObject* object) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void notifyCollisionObjectsAdded( + void addCollisionObjectsToEngine( const std::vector& collObjects) override; // Documentation inherited - void notifyCollisionObjectRemoved(CollisionObject* object) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void notifyAllCollisionObjectsRemoved() override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited void updateCollisionGroupEngineData() override; diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 4c5804fc23ef5..ffbe69ec93e49 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -612,12 +612,12 @@ bool FCLCollisionDetector::detect( { result.clear(); - if (!group) - return false; - - if (group->getCollisionDetector()->getType() - != FCLCollisionDetector::getTypeStatic()) + if (this != group->getCollisionDetector().get()) { + dterr << "[FCLCollisionDetector::detect] Attempting to check collision " + << "for a collision group that is created from a different collision " + << "detector instance.\n"; + return false; } @@ -641,19 +641,14 @@ bool FCLCollisionDetector::detect( { result.clear(); - if (!group1 || !group2) - return false; - - if (group1->getCollisionDetector()->getType() - != FCLCollisionDetector::getTypeStatic()) + if ((this != group1->getCollisionDetector().get()) + || (this != group2->getCollisionDetector().get())) { - return false; - } + dterr << "[FCLCollisionDetector::detect] Attempting to check collision " + << "for a collision group that is created from a different collision " + << "detector instance.\n"; - if (group2->getCollisionDetector()->getType() - != FCLCollisionDetector::getTypeStatic()) - { - return false; + return false; } group1->updateEngineData(); diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index 72df8f35b652d..5385c696e30c2 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -51,12 +51,6 @@ FCLCollisionGroup::FCLCollisionGroup( // Do nothing } -//============================================================================== -FCLCollisionGroup::~FCLCollisionGroup() -{ - // Do nothing -} - //============================================================================== void FCLCollisionGroup::initializeEngineData() { @@ -64,7 +58,7 @@ void FCLCollisionGroup::initializeEngineData() } //============================================================================== -void FCLCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) +void FCLCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) { auto casted = static_cast(object); mBroadPhaseAlg->registerObject(casted->getFCLCollisionObject()); @@ -73,7 +67,7 @@ void FCLCollisionGroup::notifyCollisionObjectAdded(CollisionObject* object) } //============================================================================== -void FCLCollisionGroup::notifyCollisionObjectsAdded( +void FCLCollisionGroup::addCollisionObjectsToEngine( const std::vector& collObjects) { for (auto collObj : collObjects) @@ -87,7 +81,7 @@ void FCLCollisionGroup::notifyCollisionObjectsAdded( } //============================================================================== -void FCLCollisionGroup::notifyCollisionObjectRemoved(CollisionObject* object) +void FCLCollisionGroup::removeCollisionObjectFromEngine(CollisionObject* object) { auto casted = static_cast(object); @@ -97,7 +91,7 @@ void FCLCollisionGroup::notifyCollisionObjectRemoved(CollisionObject* object) } //============================================================================== -void FCLCollisionGroup::notifyAllCollisionObjectsRemoved() +void FCLCollisionGroup::removeAllCollisionObjectsFromEngine() { mBroadPhaseAlg->clear(); diff --git a/dart/collision/fcl/FCLCollisionGroup.h b/dart/collision/fcl/FCLCollisionGroup.h index 3636446bfd8b2..f5069690139b2 100644 --- a/dart/collision/fcl/FCLCollisionGroup.h +++ b/dart/collision/fcl/FCLCollisionGroup.h @@ -59,7 +59,7 @@ class FCLCollisionGroup : public CollisionGroup FCLCollisionGroup(const CollisionDetectorPtr& collisionDetector); /// Destructor - virtual ~FCLCollisionGroup(); + virtual ~FCLCollisionGroup() = default; protected: @@ -67,17 +67,17 @@ class FCLCollisionGroup : public CollisionGroup void initializeEngineData() override; // Documentation inherited - void notifyCollisionObjectAdded(CollisionObject* object) override; + void addCollisionObjectToEngine(CollisionObject* object) override; // Documentation inherited - void notifyCollisionObjectsAdded( + void addCollisionObjectsToEngine( const std::vector& collObjects) override; // Documentation inherited - void notifyCollisionObjectRemoved(CollisionObject* object) override; + void removeCollisionObjectFromEngine(CollisionObject* object) override; // Documentation inherited - void notifyAllCollisionObjectsRemoved() override; + void removeAllCollisionObjectsFromEngine() override; // Documentation inherited void updateCollisionGroupEngineData() override; From 01c8e418e6e4107a62408de5e07046087e649dfd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 5 Apr 2016 15:09:17 -0400 Subject: [PATCH 55/67] Use map instead of parallel vectors in CollisionGroup --- dart/collision/CollisionGroup.cpp | 49 ++++--------------- dart/collision/CollisionGroup.h | 20 ++------ .../collision/bullet/BulletCollisionGroup.cpp | 4 +- dart/collision/dart/DARTCollisionDetector.cpp | 18 ++++--- dart/collision/dart/DARTCollisionGroup.cpp | 20 +++++--- dart/collision/dart/DARTCollisionGroup.h | 7 +++ dart/collision/detail/CollisionGroup.h | 8 +-- 7 files changed, 51 insertions(+), 75 deletions(-) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 5bf611dae9e6c..b0515753f486e 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -78,8 +78,7 @@ void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) addCollisionObjectToEngine(collObj.get()); - mShapeFrames.push_back(shapeFrame); - mCollisionObjects.push_back(collObj); + mShapeFrameMap[shapeFrame] = collObj; } //============================================================================== @@ -102,18 +101,14 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) if (!shapeFrame) return; - const auto search - = std::find(mShapeFrames.begin(), mShapeFrames.end(), shapeFrame); + const auto search = mShapeFrameMap.find(shapeFrame); - if (mShapeFrames.end() == search) + if (mShapeFrameMap.end() == search) return; - const size_t index = search - mShapeFrames.begin(); + removeCollisionObjectFromEngine(search->second.get()); - removeCollisionObjectFromEngine(mCollisionObjects[index].get()); - - mShapeFrames.erase(search); - mCollisionObjects.erase(mCollisionObjects.begin() + index); + mShapeFrameMap.erase(search); } //============================================================================== @@ -135,37 +130,20 @@ void CollisionGroup::removeAllShapeFrames() { removeAllCollisionObjectsFromEngine(); - mShapeFrames.clear(); - mCollisionObjects.clear(); + mShapeFrameMap.clear(); } //============================================================================== bool CollisionGroup::hasShapeFrame( const dynamics::ShapeFrame* shapeFrame) const { - return std::find_if( - mShapeFrames.begin(), mShapeFrames.end(), - [&](const dynamics::ShapeFrame* it) { return it == shapeFrame; }) - != mShapeFrames.end(); + return mShapeFrameMap.find(shapeFrame) != mShapeFrameMap.end(); } //============================================================================== size_t CollisionGroup::getNumShapeFrames() const { - return mShapeFrames.size(); -} - -//============================================================================== -const dynamics::ShapeFrame* CollisionGroup::getShapeFrame(size_t index) const -{ - return common::getVectorObjectIfAvailable(index, mShapeFrames); -} - -//============================================================================== -const std::vector& -CollisionGroup::getShapeFrames() const -{ - return mShapeFrames; + return mShapeFrameMap.size(); } //============================================================================== @@ -181,18 +159,11 @@ bool CollisionGroup::detect( return mCollisionDetector->detect(this, other, option, result); } -//============================================================================== -const std::vector>& -CollisionGroup::getCollisionObjects() -{ - return mCollisionObjects; -} - //============================================================================== void CollisionGroup::updateEngineData() { - for (auto& object : mCollisionObjects) - object->updateEngineData(); + for (auto& pair : mShapeFrameMap) + pair.second->updateEngineData(); updateCollisionGroupEngineData(); } diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index d9d578fc1074a..285dce9884f0c 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -37,6 +37,7 @@ #ifndef DART_COLLISION_COLLISIONGROUP_H_ #define DART_COLLISION_COLLISIONGROUP_H_ +#include #include #include "dart/collision/SmartPointer.h" #include "dart/collision/Option.h" @@ -170,13 +171,6 @@ class CollisionGroup /// Return number of ShapeFrames added to this CollisionGroup size_t getNumShapeFrames() const; - /// Return index-th ShapeFrames added to this CollisionGroup. Return nullptr - /// if the index is out of the range. - const dynamics::ShapeFrame* getShapeFrame(size_t index) const; - - /// Return all the ShapeFrames added to this CollisionGroup - const std::vector& getShapeFrames() const; - /// Perform collision detection within this CollisionGroup. bool detect(const Option& option, Result& result); @@ -186,9 +180,6 @@ class CollisionGroup /// from this CollisionObject engine. bool detect(CollisionGroup* group, const Option& option, Result& result); - /// Return all the CollisionObjects in this CollisionGroup - const std::vector >& getCollisionObjects(); - protected: /// Update engine data. This function should be called before the collision @@ -217,7 +208,7 @@ class CollisionGroup /// This function will be called ahead of every collision checking. virtual void updateCollisionGroupEngineData() = 0; -private: +protected: /// Collision detector CollisionDetectorPtr mCollisionDetector; @@ -226,11 +217,8 @@ class CollisionGroup // CollisionDetector doesn't get destroyed as long as at least one // CollisionGroup is alive. - /// ShapeFrames added to this CollisionGroup - std::vector mShapeFrames; - - /// CollisionObjects associated with the added ShapeFrames - std::vector> mCollisionObjects; + /// ShapeFrames and CollisionOjbects added to this CollisionGroup + std::map mShapeFrameMap; // CollisionGroup also shares the ownership of CollisionObjects across other // CollisionGroups for the same reason with above. diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 916b2254e9768..71dacede2c82a 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -104,8 +104,8 @@ void BulletCollisionGroup::removeCollisionObjectFromEngine( //============================================================================== void BulletCollisionGroup::removeAllCollisionObjectsFromEngine() { - for (const auto& collisionObject : getCollisionObjects()) - removeCollisionObjectFromEngine(collisionObject.get()); + for (const auto& pair : mShapeFrameMap) + removeCollisionObjectFromEngine(pair.second.get()); initializeEngineData(); } diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 61b64ec1861b1..64f9b62ebaa4e 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -104,7 +104,8 @@ bool DARTCollisionDetector::detect( return false; } - auto objects = group->getCollisionObjects(); + auto casted = static_cast(group); + const auto& objects = casted->mCollisionObjects; if (objects.empty()) return false; @@ -120,10 +121,10 @@ bool DARTCollisionDetector::detect( { auto collObj2 = objects[j]; - if (filter && !filter->needCollision(collObj1.get(), collObj2.get())) + if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1.get(), collObj2.get(), option, result); + checkPair(collObj1, collObj2, option, result); if ((option.binaryCheck && result.isCollision()) || (result.getNumContacts() >= option.maxNumContacts)) @@ -158,8 +159,11 @@ bool DARTCollisionDetector::detect( return false; } - auto objects1 = group1->getCollisionObjects(); - auto objects2 = group2->getCollisionObjects(); + auto casted1 = static_cast(group1); + auto casted2 = static_cast(group2); + + const auto& objects1 = casted1->mCollisionObjects; + const auto& objects2 = casted2->mCollisionObjects; if (objects1.empty() || objects2.empty()) return false; @@ -175,10 +179,10 @@ bool DARTCollisionDetector::detect( { auto collObj2 = objects2[j]; - if (filter && !filter->needCollision(collObj1.get(), collObj2.get())) + if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1.get(), collObj2.get(), option, result); + checkPair(collObj1, collObj2, option, result); if (result.getNumContacts() >= option.maxNumContacts) { diff --git a/dart/collision/dart/DARTCollisionGroup.cpp b/dart/collision/dart/DARTCollisionGroup.cpp index 2c270d6e57526..962e75c0a514c 100644 --- a/dart/collision/dart/DARTCollisionGroup.cpp +++ b/dart/collision/dart/DARTCollisionGroup.cpp @@ -56,29 +56,35 @@ void DARTCollisionGroup::initializeEngineData() } //============================================================================== -void DARTCollisionGroup::addCollisionObjectToEngine(CollisionObject* /*object*/) +void DARTCollisionGroup::addCollisionObjectToEngine(CollisionObject* object) { - // Do nothing + if (std::find(mCollisionObjects.begin(), mCollisionObjects.end(), object) + == mCollisionObjects.end()) + { + mCollisionObjects.push_back(object); + } } //============================================================================== void DARTCollisionGroup::addCollisionObjectsToEngine( - const std::vector& /*collObjects*/) + const std::vector& collObjects) { - // Do nothing + for (auto collObject : collObjects) + addCollisionObjectToEngine(collObject); } //============================================================================== void DARTCollisionGroup::removeCollisionObjectFromEngine( - CollisionObject* /*object*/) + CollisionObject* object) { - // Do nothing + mCollisionObjects.erase( + std::remove(mCollisionObjects.begin(), mCollisionObjects.end(), object)); } //============================================================================== void DARTCollisionGroup::removeAllCollisionObjectsFromEngine() { - // Do nothing + mCollisionObjects.clear(); } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionGroup.h b/dart/collision/dart/DARTCollisionGroup.h index ae33c7d8c37f6..3613c0583eff1 100644 --- a/dart/collision/dart/DARTCollisionGroup.h +++ b/dart/collision/dart/DARTCollisionGroup.h @@ -48,6 +48,8 @@ class DARTCollisionGroup : public CollisionGroup { public: + friend class DARTCollisionDetector; + /// Constructor DARTCollisionGroup(const CollisionDetectorPtr& collisionDetector); @@ -75,6 +77,11 @@ class DARTCollisionGroup : public CollisionGroup // Documentation inherited void updateCollisionGroupEngineData() override; +protected: + + /// CollisionObjects added to this DARTCollisionGroup + std::vector mCollisionObjects; + }; } // namespace collision diff --git a/dart/collision/detail/CollisionGroup.h b/dart/collision/detail/CollisionGroup.h index 81efad033f4df..5eab0693b76b4 100644 --- a/dart/collision/detail/CollisionGroup.h +++ b/dart/collision/detail/CollisionGroup.h @@ -75,8 +75,8 @@ void CollisionGroup::addShapeFramesOf( if (otherGroup && this != otherGroup) { - for (const auto& shapeFrame : otherGroup->mShapeFrames) - addShapeFrame(shapeFrame); + for (const auto& pair : otherGroup->mShapeFrameMap) + addShapeFrame(pair.first); } addShapeFramesOf(others...); @@ -148,8 +148,8 @@ void CollisionGroup::removeShapeFramesOf( return; } - for (const auto& shapeFrame : otherGroup->mShapeFrames) - removeShapeFrame(shapeFrame); + for (const auto& pair : otherGroup->mShapeFrameMap) + removeShapeFrame(pair.first); } removeShapeFramesOf(others...); From 8927ebb4d7a674353c292538b6c860b815439d0f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 6 Apr 2016 14:24:51 -0400 Subject: [PATCH 56/67] Fix bug in FCLCollisionCallbackData --- dart/collision/fcl/FCLCollisionDetector.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index ffbe69ec93e49..9e7081bf2df54 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -123,7 +123,7 @@ struct FCLCollisionCallbackData const Option& mOption; /// Collision result of DART - Result* mResult; + Result& mResult; FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType; @@ -136,7 +136,7 @@ struct FCLCollisionCallbackData /// Constructor FCLCollisionCallbackData( const Option& option, - Result* result = nullptr, + Result& result, FCLCollisionDetector::PrimitiveShape type = FCLCollisionDetector::MESH, FCLCollisionDetector::ContactPointComputationMethod method @@ -627,7 +627,7 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg = castedData->getFCLCollisionManager(); FCLCollisionCallbackData collData( - option, &result, mPrimitiveShapeType, + option, result, mPrimitiveShapeType, mContactPointComputationMethod); broadPhaseAlg->collide(&collData, collisionCallback); @@ -661,7 +661,7 @@ bool FCLCollisionDetector::detect( auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); FCLCollisionCallbackData collData( - &option, &result, mPrimitiveShapeType, + option, result, mPrimitiveShapeType, mContactPointComputationMethod); broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); @@ -938,7 +938,7 @@ bool collisionCallback( const auto& fclRequest = collData->mFclRequest; auto& fclResult = collData->mFclResult; - auto& result = *(collData->mResult); + auto& result = collData->mResult; const auto& option = collData->mOption; auto filter = option.collisionFilter; From df324db5758211c5441cd7d928f41f8e063bce0e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 6 Apr 2016 17:02:39 -0400 Subject: [PATCH 57/67] Rollback the replacing parallel vectors with map in CollisionGroup (see the comment for the detail) --- dart/collision/CollisionGroup.cpp | 14 +++++++++++--- dart/collision/CollisionGroup.h | 13 ++++++++++++- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index b0515753f486e..561028913550f 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -78,7 +78,7 @@ void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) addCollisionObjectToEngine(collObj.get()); - mShapeFrameMap[shapeFrame] = collObj; + mShapeFrameMap.push_back(std::make_pair(shapeFrame, collObj)); } //============================================================================== @@ -101,7 +101,11 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) if (!shapeFrame) return; - const auto search = mShapeFrameMap.find(shapeFrame); + const auto search + = std::find_if(mShapeFrameMap.begin(), mShapeFrameMap.end(), + [&](const std::pair& pair) + { return pair.first == shapeFrame; }); if (mShapeFrameMap.end() == search) return; @@ -137,7 +141,11 @@ void CollisionGroup::removeAllShapeFrames() bool CollisionGroup::hasShapeFrame( const dynamics::ShapeFrame* shapeFrame) const { - return mShapeFrameMap.find(shapeFrame) != mShapeFrameMap.end(); + return std::find_if(mShapeFrameMap.begin(), mShapeFrameMap.end(), + [&](const std::pair& pair) + { return pair.first == shapeFrame; }) + != mShapeFrameMap.end(); } //============================================================================== diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 285dce9884f0c..d199fd3c996ca 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -218,9 +218,20 @@ class CollisionGroup // CollisionGroup is alive. /// ShapeFrames and CollisionOjbects added to this CollisionGroup - std::map mShapeFrameMap; + std::vector> mShapeFrameMap; // CollisionGroup also shares the ownership of CollisionObjects across other // CollisionGroups for the same reason with above. + // + // Dev note: This was supposed to be std::map rather than std::vector for + // better search performance. The reason we use std::vector is to get + // deterministic contact results regardless of the order of CollisionObjects + // in this container for FCLCollisionDetector. + // + // fcl's collision result is dependent on the order of objects in the broad + // phase classes. If we use std::map, the orders of element between the + // original and copy are not guranteed to be the same as we copy std::map + // (e.g., by world cloning). }; From 9070bc968c975b652ca019d45c52988137f97164 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 6 Apr 2016 18:07:38 -0400 Subject: [PATCH 58/67] FCL 0.4 should work now by df324db --- ci/install_osx.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ci/install_osx.sh b/ci/install_osx.sh index 3a3357142d204..e80330ad26dc9 100755 --- a/ci/install_osx.sh +++ b/ci/install_osx.sh @@ -7,7 +7,7 @@ PACKAGES=' git cmake assimp -fcl03 +fcl bullet flann boost From 79604162976001c21ae7e98be80331905e84abe5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 7 Apr 2016 15:34:37 -0400 Subject: [PATCH 59/67] Rename CollisionDetector::detect() to collide() considering adding distance() method in the future --- dart/collision/CollisionDetector.h | 8 ++++---- dart/collision/bullet/BulletCollisionDetector.cpp | 6 ++++-- dart/collision/bullet/BulletCollisionDetector.h | 8 ++++---- dart/collision/dart/DARTCollisionDetector.cpp | 4 ++-- dart/collision/dart/DARTCollisionDetector.h | 8 ++++---- dart/collision/fcl/FCLCollisionDetector.cpp | 4 ++-- dart/collision/fcl/FCLCollisionDetector.h | 8 ++++---- 7 files changed, 24 insertions(+), 22 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index a024ce6c38452..9ac9e87951536 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -95,12 +95,12 @@ class CollisionDetector : public std::enable_shared_from_this const Args&... args); /// Perform collision detection for group. - virtual bool detect(CollisionGroup* group, - const Option& option, Result& result) = 0; + virtual bool collide(CollisionGroup* group, + const Option& option, Result& result) = 0; /// Perform collision detection for group1-group2. - virtual bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) = 0; + virtual bool collide(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) = 0; protected: diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 8b25d673e24a1..2c94b1fc6127e 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -171,7 +171,7 @@ BulletCollisionDetector::createCollisionGroup() } //============================================================================== -bool BulletCollisionDetector::detect( +bool BulletCollisionDetector::collide( CollisionGroup* group, const Option& option, Result& result) { result.clear(); @@ -192,6 +192,8 @@ bool BulletCollisionDetector::detect( auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); + bulletCollisionWorld->getPairCache()->processAllOverlappingPairs(); + bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); @@ -201,7 +203,7 @@ bool BulletCollisionDetector::detect( } //============================================================================== -bool BulletCollisionDetector::detect( +bool BulletCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index 04793520a7ffa..f546f446b2bbe 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -68,12 +68,12 @@ class BulletCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; + bool collide(CollisionGroup* group, + const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; + bool collide(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; protected: diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 64f9b62ebaa4e..94cb67307a6f4 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -89,7 +89,7 @@ DARTCollisionDetector::createCollisionGroup() } //============================================================================== -bool DARTCollisionDetector::detect( +bool DARTCollisionDetector::collide( CollisionGroup* group, const Option& option, Result& result) { @@ -142,7 +142,7 @@ bool DARTCollisionDetector::detect( } //============================================================================== -bool DARTCollisionDetector::detect( +bool DARTCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 17d1036cab4af..9837142ee6fe0 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -61,12 +61,12 @@ class DARTCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; + bool collide(CollisionGroup* group, + const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; + bool collide(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; protected: diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 9e7081bf2df54..71638bfe43dfe 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -607,7 +607,7 @@ FCLCollisionDetector::createCollisionGroup() } //============================================================================== -bool FCLCollisionDetector::detect( +bool FCLCollisionDetector::collide( CollisionGroup* group, const Option& option, Result& result) { result.clear(); @@ -635,7 +635,7 @@ bool FCLCollisionDetector::detect( } //============================================================================== -bool FCLCollisionDetector::detect( +bool FCLCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, const Option& option, Result& result) { diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index 7b3ae72655a27..adbc8294f6056 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -98,12 +98,12 @@ class FCLCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool detect(CollisionGroup* group, - const Option& option, Result& result) override; + bool collide(CollisionGroup* group, + const Option& option, Result& result) override; // Documentation inherited - bool detect(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; + bool collide(CollisionGroup* group1, CollisionGroup* group2, + const Option& option, Result& result) override; /// Set primitive shape type void setPrimitiveShapeType(PrimitiveShape type); From f36a12c3d21fdcbf79827abf79033ae08d9b02f6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 7 Apr 2016 15:40:38 -0400 Subject: [PATCH 60/67] Apply the previous renaming to other code --- dart/collision/CollisionGroup.cpp | 8 ++--- dart/collision/CollisionGroup.h | 4 +-- .../bullet/BulletCollisionDetector.cpp | 2 -- dart/constraint/ConstraintSolver.cpp | 2 +- dart/simulation/World.cpp | 2 +- tutorials/tutorialCollisions-Finished.cpp | 2 +- tutorials/tutorialDominoes-Finished.cpp | 2 +- unittests/testCollision.cpp | 30 +++++++++---------- 8 files changed, 25 insertions(+), 27 deletions(-) diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 561028913550f..722574ac80e00 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -155,16 +155,16 @@ size_t CollisionGroup::getNumShapeFrames() const } //============================================================================== -bool CollisionGroup::detect(const Option& option, Result& result) +bool CollisionGroup::collide(const Option& option, Result& result) { - return mCollisionDetector->detect(this, option, result); + return mCollisionDetector->collide(this, option, result); } //============================================================================== -bool CollisionGroup::detect( +bool CollisionGroup::collide( CollisionGroup* other, const Option& option, Result& result) { - return mCollisionDetector->detect(this, other, option, result); + return mCollisionDetector->collide(this, other, option, result); } //============================================================================== diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index d199fd3c996ca..5703d71ad582c 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -172,13 +172,13 @@ class CollisionGroup size_t getNumShapeFrames() const; /// Perform collision detection within this CollisionGroup. - bool detect(const Option& option, Result& result); + bool collide(const Option& option, Result& result); /// Perform collision detection with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool detect(CollisionGroup* group, const Option& option, Result& result); + bool collide(CollisionGroup* group, const Option& option, Result& result); protected: diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 2c94b1fc6127e..e528c0bfe39f2 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -192,8 +192,6 @@ bool BulletCollisionDetector::collide( auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); - bulletCollisionWorld->getPairCache()->processAllOverlappingPairs(); - bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 4ada32e6d7038..c523c586b6cc0 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -381,7 +381,7 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- mCollisionResult.clear(); - mCollisionGroup->detect(mCollisionOption, mCollisionResult); + mCollisionGroup->collide(mCollisionOption, mCollisionResult); // Destroy previous contact constraints mContactConstraints.clear(); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 4e97faa7ba4ae..28fc1a5c8e7ff 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -499,7 +499,7 @@ bool World::checkCollision(bool checkAllCollisions) collision::Result result; - return mConstraintSolver->getCollisionGroup()->detect(option, result); + return mConstraintSolver->getCollisionGroup()->collide(option, result); } //============================================================================== diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 0db3dfd606a23..c2620b80aa43d 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -219,7 +219,7 @@ class MyWindow : public dart::gui::SimWindow dart::collision::Option option; dart::collision::Result result; - bool collision = collisionGroup->detect(newGroup.get(), option, result); + bool collision = collisionGroup->collide(newGroup.get(), option, result); // If the new object is not in collision if(!collision) diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 61d2a45490195..c9ba7c0980ee0 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -271,7 +271,7 @@ class MyWindow : public dart::gui::SimWindow dart::collision::Option option; dart::collision::Result result; bool dominoCollision - = collisionGroup->detect(newGroup.get(), option, result); + = collisionGroup->collide(newGroup.get(), option, result); // If the new domino is not penetrating an existing one if(!dominoCollision) diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 22ce66ccb6e63..67ba11f17f84c 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -547,18 +547,18 @@ void testSimpleFrames(const std::shared_ptr& cd) simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); - EXPECT_FALSE(group1->detect(option, result)); - EXPECT_FALSE(group2->detect(option, result)); - EXPECT_FALSE(group3->detect(option, result)); - EXPECT_FALSE(groupAll->detect(option, result)); + EXPECT_FALSE(group1->collide(option, result)); + EXPECT_FALSE(group2->collide(option, result)); + EXPECT_FALSE(group3->collide(option, result)); + EXPECT_FALSE(groupAll->collide(option, result)); simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); - EXPECT_TRUE(group1->detect(group2.get(), option, result)); - EXPECT_TRUE(group1->detect(group2.get(), option, result)); - EXPECT_TRUE(group2->detect(group3.get(), option, result)); - EXPECT_TRUE(groupAll->detect(option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, result)); + EXPECT_TRUE(group2->collide(group3.get(), option, result)); + EXPECT_TRUE(groupAll->collide(option, result)); } //============================================================================== @@ -636,7 +636,7 @@ void testBoxBox(const std::shared_ptr& cd, collision::Option option; collision::Result result; - EXPECT_TRUE(group1->detect(group2.get(), option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, result)); Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); @@ -722,20 +722,20 @@ void testOptions(const std::shared_ptr& cd) result.clear(); option.maxNumContacts = 1000u; option.binaryCheck = false; - EXPECT_TRUE(group->detect(option, result)); + EXPECT_TRUE(group->collide(option, result)); EXPECT_EQ(result.getNumContacts(), 4u); result.clear(); option.maxNumContacts = 2u; option.binaryCheck = false; - EXPECT_TRUE(group->detect(option, result)); + EXPECT_TRUE(group->collide(option, result)); EXPECT_EQ(result.getNumContacts(), 2u); group->addShapeFrame(simpleFrame3.get()); result.clear(); option.maxNumContacts = 1e+3; option.binaryCheck = true; - EXPECT_TRUE(group->detect(option, result)); + EXPECT_TRUE(group->collide(option, result)); EXPECT_EQ(result.getNumContacts(), 1u); } @@ -799,9 +799,9 @@ void testCreateCollisionGroups(const std::shared_ptr& cd) auto shapeNodeGroup1 = cd->createCollisionGroup(boxShapeNode1); auto shapeNodeGroup2 = cd->createCollisionGroup(boxShapeNode2); - EXPECT_TRUE(skeletonGroup1->detect(skeletonGroup2.get(), option, result)); - EXPECT_TRUE(bodyNodeGroup1->detect(bodyNodeGroup2.get(), option, result)); - EXPECT_TRUE(shapeNodeGroup1->detect(shapeNodeGroup2.get(), option, result)); + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option, result)); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, result)); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, result)); } //============================================================================== From 5a1652bdf1d003a4260fd00c1341d36ff83e8838 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 7 Apr 2016 22:46:45 -0400 Subject: [PATCH 61/67] Install files under 'dart/collision/detail' directory --- dart/collision/CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/dart/collision/CMakeLists.txt b/dart/collision/CMakeLists.txt index e80d01296c9d7..9db930d4e0789 100644 --- a/dart/collision/CMakeLists.txt +++ b/dart/collision/CMakeLists.txt @@ -56,5 +56,12 @@ install( DESTINATION include/dart/collision COMPONENT headers ) + +install( + FILES ${detail_hdrs} + DESTINATION include/dart/collision/detail + COMPONENT headers +) + #install(TARGETS dart_collision EXPORT DARTCoreTargets DESTINATION lib) #install(TARGETS dart_collision EXPORT DARTTargets DESTINATION lib) From 540802aaa433890a365f2ad5c322124ad8f30d5a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 8 Apr 2016 03:05:48 -0400 Subject: [PATCH 62/67] Rename Option/Result to CollisionOption/CollisionResult --- dart/collision/CollisionDetector.h | 4 +-- dart/collision/CollisionGroup.cpp | 4 +-- dart/collision/CollisionGroup.h | 4 +-- dart/collision/Option.cpp | 2 +- dart/collision/Option.h | 4 +-- dart/collision/Result.cpp | 14 ++++---- dart/collision/Result.h | 2 +- .../bullet/BulletCollisionDetector.cpp | 14 ++++---- .../bullet/BulletCollisionDetector.h | 4 +-- dart/collision/dart/DARTCollide.cpp | 16 +++++----- dart/collision/dart/DARTCollide.h | 14 ++++---- dart/collision/dart/DARTCollisionDetector.cpp | 18 +++++------ dart/collision/dart/DARTCollisionDetector.h | 4 +-- dart/collision/fcl/FCLCollisionDetector.cpp | 32 +++++++++---------- dart/collision/fcl/FCLCollisionDetector.h | 4 +-- dart/constraint/ConstraintSolver.cpp | 6 ++-- dart/constraint/ConstraintSolver.h | 8 ++--- dart/simulation/World.cpp | 4 +-- tutorials/tutorialCollisions-Finished.cpp | 4 +-- tutorials/tutorialDominoes-Finished.cpp | 4 +-- unittests/testCollision.cpp | 16 +++++----- 21 files changed, 91 insertions(+), 91 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 9ac9e87951536..96f3bdfc72242 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -96,11 +96,11 @@ class CollisionDetector : public std::enable_shared_from_this /// Perform collision detection for group. virtual bool collide(CollisionGroup* group, - const Option& option, Result& result) = 0; + const CollisionOption& option, CollisionResult& result) = 0; /// Perform collision detection for group1-group2. virtual bool collide(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) = 0; + const CollisionOption& option, CollisionResult& result) = 0; protected: diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 722574ac80e00..d2e3f8de617eb 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -155,14 +155,14 @@ size_t CollisionGroup::getNumShapeFrames() const } //============================================================================== -bool CollisionGroup::collide(const Option& option, Result& result) +bool CollisionGroup::collide(const CollisionOption& option, CollisionResult& result) { return mCollisionDetector->collide(this, option, result); } //============================================================================== bool CollisionGroup::collide( - CollisionGroup* other, const Option& option, Result& result) + CollisionGroup* other, const CollisionOption& option, CollisionResult& result) { return mCollisionDetector->collide(this, other, option, result); } diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index 5703d71ad582c..a9f272b2cf5d7 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -172,13 +172,13 @@ class CollisionGroup size_t getNumShapeFrames() const; /// Perform collision detection within this CollisionGroup. - bool collide(const Option& option, Result& result); + bool collide(const CollisionOption& option, CollisionResult& result); /// Perform collision detection with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool collide(CollisionGroup* group, const Option& option, Result& result); + bool collide(CollisionGroup* group, const CollisionOption& option, CollisionResult& result); protected: diff --git a/dart/collision/Option.cpp b/dart/collision/Option.cpp index b9289a95386fd..113652cec7880 100644 --- a/dart/collision/Option.cpp +++ b/dart/collision/Option.cpp @@ -40,7 +40,7 @@ namespace dart { namespace collision { //============================================================================== -Option::Option( +CollisionOption::CollisionOption( bool enableContact, bool binaryCheck, size_t maxNumContacts, diff --git a/dart/collision/Option.h b/dart/collision/Option.h index 71466b2eef561..3371a6a78fa3d 100644 --- a/dart/collision/Option.h +++ b/dart/collision/Option.h @@ -45,7 +45,7 @@ namespace collision { class CollisionFilter; -struct Option +struct CollisionOption { /// Flag whether compute contact information such as point, normal, and /// penetration depth. If this flag is set to false, the Engine returns only @@ -63,7 +63,7 @@ struct Option std::shared_ptr collisionFilter; /// Constructor - Option(bool enableContact = true, + CollisionOption(bool enableContact = true, bool binaryCheck = false, size_t maxNumContacts = 100, const std::shared_ptr& collisionFilter = nullptr); diff --git a/dart/collision/Result.cpp b/dart/collision/Result.cpp index d01fc759a70c1..55333262fb879 100644 --- a/dart/collision/Result.cpp +++ b/dart/collision/Result.cpp @@ -40,19 +40,19 @@ namespace dart { namespace collision { //============================================================================== -void Result::addContact(const Contact& contact) +void CollisionResult::addContact(const Contact& contact) { mContacts.push_back(contact); } //============================================================================== -size_t Result::getNumContacts() const +size_t CollisionResult::getNumContacts() const { return mContacts.size(); } //============================================================================== -Contact& Result::getContact(size_t index) +Contact& CollisionResult::getContact(size_t index) { assert(index < mContacts.size()); @@ -60,7 +60,7 @@ Contact& Result::getContact(size_t index) } //============================================================================== -const Contact& Result::getContact(size_t index) const +const Contact& CollisionResult::getContact(size_t index) const { assert(index < mContacts.size()); @@ -68,19 +68,19 @@ const Contact& Result::getContact(size_t index) const } //============================================================================== -const std::vector& Result::getContacts() const +const std::vector& CollisionResult::getContacts() const { return mContacts; } //============================================================================== -bool Result::isCollision() const +bool CollisionResult::isCollision() const { return !mContacts.empty(); } //============================================================================== -void Result::clear() +void CollisionResult::clear() { mContacts.clear(); } diff --git a/dart/collision/Result.h b/dart/collision/Result.h index 544d13d4b0576..4e3ded8970193 100644 --- a/dart/collision/Result.h +++ b/dart/collision/Result.h @@ -43,7 +43,7 @@ namespace dart { namespace collision { -class Result +class CollisionResult { public: diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index e528c0bfe39f2..d2536c91dae20 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -60,7 +60,7 @@ namespace { struct BulletOverlapFilterCallback : public btOverlapFilterCallback { - BulletOverlapFilterCallback(const Option& option, const Result& result) + BulletOverlapFilterCallback(const CollisionOption& option, const CollisionResult& result) : mOption(option), mResult(result), mDone(false) @@ -111,8 +111,8 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback return collide; } - const Option& mOption; - const Result& mResult; + const CollisionOption& mOption; + const CollisionResult& mResult; /// Whether the collision iteration can stop mutable bool mDone; @@ -123,7 +123,7 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData2); void convertContacts( - btCollisionWorld* collWorld, const Option& option, Result& result); + btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result); btCollisionShape* createBulletEllipsoidMesh(float sizeX, float sizeY, float sizeZ); @@ -172,7 +172,7 @@ BulletCollisionDetector::createCollisionGroup() //============================================================================== bool BulletCollisionDetector::collide( - CollisionGroup* group, const Option& option, Result& result) + CollisionGroup* group, const CollisionOption& option, CollisionResult& result) { result.clear(); @@ -203,7 +203,7 @@ bool BulletCollisionDetector::collide( //============================================================================== bool BulletCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) + const CollisionOption& option, CollisionResult& result) { result.clear(); @@ -445,7 +445,7 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, //============================================================================== void convertContacts( - btCollisionWorld* collWorld, const Option& option, Result& result) + btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result) { assert(collWorld); diff --git a/dart/collision/bullet/BulletCollisionDetector.h b/dart/collision/bullet/BulletCollisionDetector.h index f546f446b2bbe..61711ff32bb8e 100644 --- a/dart/collision/bullet/BulletCollisionDetector.h +++ b/dart/collision/bullet/BulletCollisionDetector.h @@ -69,11 +69,11 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited bool collide(CollisionGroup* group, - const Option& option, Result& result) override; + const CollisionOption& option, CollisionResult& result) override; // Documentation inherited bool collide(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; + const CollisionOption& option, CollisionResult& result) override; protected: diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 378849a882b7c..9e7da65946a12 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -419,7 +419,7 @@ void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, // fields. int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, const dVector3 p2, const dMatrix3 R2, const dVector3 side2, - Result& result) + CollisionResult& result) { const double fudge_factor = 1.05; dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0}; @@ -802,7 +802,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - Result& result) + CollisionResult& result) { dVector3 halfSize0; dVector3 halfSize1; @@ -826,7 +826,7 @@ int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const double& r1, const Eigen::Isometry3d& T1, - Result& result) + CollisionResult& result) { Eigen::Vector3d halfSize = 0.5 * size0; bool inside_box = true; @@ -935,7 +935,7 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - Result& result) + CollisionResult& result) { Eigen::Vector3d size = 0.5 * size1; bool inside_box = true; @@ -1040,7 +1040,7 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, const double& _r1, const Eigen::Isometry3d& c1, - Result& result) + CollisionResult& result) { double r0 = _r0; double r1 = _r1; @@ -1087,7 +1087,7 @@ int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, int collideCylinderSphere(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const double& sphere_rad, const Eigen::Isometry3d& T1, - Result& result) + CollisionResult& result) { Eigen::Vector3d center = T0.inverse() * T1.translation(); @@ -1149,7 +1149,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1, - Result& result) + CollisionResult& result) { Eigen::Vector3d normal = T1.linear() * plane_normal; Eigen::Vector3d Rx = T0.linear().rightCols(1); @@ -1219,7 +1219,7 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const //============================================================================== int collide(dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, dynamics::ConstShapePtr shape1, const Eigen::Isometry3d& T1, - Result& result) + CollisionResult& result) { dynamics::Shape::ShapeType LeftType = shape0->getShapeType(); dynamics::Shape::ShapeType RightType = shape1->getShapeType(); diff --git a/dart/collision/dart/DARTCollide.h b/dart/collision/dart/DARTCollide.h index 07887dbb66e8a..4a676c0c168a2 100644 --- a/dart/collision/dart/DARTCollide.h +++ b/dart/collision/dart/DARTCollide.h @@ -55,35 +55,35 @@ namespace collision { int collide(dart::dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, dart::dynamics::ConstShapePtr shape1, const Eigen::Isometry3d& T1, - Result& result); + CollisionResult& result); int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - Result& result); + CollisionResult& result); int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const double& r1, const Eigen::Isometry3d& T1, - Result& result); + CollisionResult& result); int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, - Result& result); + CollisionResult& result); int collideSphereSphere(const double& r0, const Eigen::Isometry3d& c0, const double& r1, const Eigen::Isometry3d& c1, - Result& result); + CollisionResult& result); int collideCylinderSphere( const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const double& sphere_rad, const Eigen::Isometry3d& T1, - Result& result); + CollisionResult& result); int collideCylinderPlane( const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1, - Result& result); + CollisionResult& result); } // namespace collision } // namespace dart diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 94cb67307a6f4..a5c139e171870 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -50,13 +50,13 @@ namespace collision { namespace { bool checkPair(CollisionObject* o1, CollisionObject* o2, - const Option& option, Result& result); + const CollisionOption& option, CollisionResult& result); bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, double tol); -void postProcess(CollisionObject* o1, CollisionObject* o2, const Option& option, - Result& totalResult, const Result& pairResult); +void postProcess(CollisionObject* o1, CollisionObject* o2, const CollisionOption& option, + CollisionResult& totalResult, const CollisionResult& pairResult); } // anonymous namespace @@ -91,7 +91,7 @@ DARTCollisionDetector::createCollisionGroup() //============================================================================== bool DARTCollisionDetector::collide( CollisionGroup* group, - const Option& option, Result& result) + const CollisionOption& option, CollisionResult& result) { result.clear(); @@ -145,7 +145,7 @@ bool DARTCollisionDetector::collide( bool DARTCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) + const CollisionOption& option, CollisionResult& result) { result.clear(); @@ -249,9 +249,9 @@ namespace { //============================================================================== bool checkPair(CollisionObject* o1, CollisionObject* o2, - const Option& option, Result& result) + const CollisionOption& option, CollisionResult& result) { - Result pairResult; + CollisionResult pairResult; // Perform narrow-phase detection auto colliding = collide(o1->getShape(), o1->getTransform(), @@ -272,8 +272,8 @@ bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, //============================================================================== void postProcess(CollisionObject* o1, CollisionObject* o2, - const Option& option, - Result& totalResult, const Result& pairResult) + const CollisionOption& option, + CollisionResult& totalResult, const CollisionResult& pairResult) { if (!pairResult.isCollision()) return; diff --git a/dart/collision/dart/DARTCollisionDetector.h b/dart/collision/dart/DARTCollisionDetector.h index 9837142ee6fe0..b1b23a0d38911 100644 --- a/dart/collision/dart/DARTCollisionDetector.h +++ b/dart/collision/dart/DARTCollisionDetector.h @@ -62,11 +62,11 @@ class DARTCollisionDetector : public CollisionDetector // Documentation inherited bool collide(CollisionGroup* group, - const Option& option, Result& result) override; + const CollisionOption& option, CollisionResult& result) override; // Documentation inherited bool collide(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; + const CollisionOption& option, CollisionResult& result) override; protected: diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 71638bfe43dfe..3fff891bae148 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -75,14 +75,14 @@ bool collisionCallback(fcl::CollisionObject* o1, void postProcessFCL(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, - const Option& option, - Result& result); + const CollisionOption& option, + CollisionResult& result); void postProcessDART(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, - const Option& option, - Result& result); + const CollisionOption& option, + CollisionResult& result); int evalContactPosition(const fcl::Contact& fclContact, const fcl::BVHModel* mesh1, @@ -103,7 +103,7 @@ int FFtest( double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); -void convertOption(const Option& fclOption, fcl::CollisionRequest& request); +void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& request); Contact convertContact(const fcl::Contact& fclContact, fcl::CollisionObject* o1, @@ -120,10 +120,10 @@ struct FCLCollisionCallbackData fcl::CollisionResult mFclResult; /// Collision option of DART - const Option& mOption; + const CollisionOption& mOption; /// Collision result of DART - Result& mResult; + CollisionResult& mResult; FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType; @@ -135,8 +135,8 @@ struct FCLCollisionCallbackData /// Constructor FCLCollisionCallbackData( - const Option& option, - Result& result, + const CollisionOption& option, + CollisionResult& result, FCLCollisionDetector::PrimitiveShape type = FCLCollisionDetector::MESH, FCLCollisionDetector::ContactPointComputationMethod method @@ -608,7 +608,7 @@ FCLCollisionDetector::createCollisionGroup() //============================================================================== bool FCLCollisionDetector::collide( - CollisionGroup* group, const Option& option, Result& result) + CollisionGroup* group, const CollisionOption& option, CollisionResult& result) { result.clear(); @@ -637,7 +637,7 @@ bool FCLCollisionDetector::collide( //============================================================================== bool FCLCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) + const CollisionOption& option, CollisionResult& result) { result.clear(); @@ -991,8 +991,8 @@ bool collisionCallback( void postProcessFCL(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, - const Option& option, - Result& result) + const CollisionOption& option, + CollisionResult& result) { auto numContacts = fclResult.numContacts(); @@ -1083,8 +1083,8 @@ void postProcessFCL(const fcl::CollisionResult& fclResult, void postProcessDART(const fcl::CollisionResult& fclResult, fcl::CollisionObject* o1, fcl::CollisionObject* o2, - const Option& option, - Result& result) + const CollisionOption& option, + CollisionResult& result) { auto initNumContacts = fclResult.numContacts(); @@ -1333,7 +1333,7 @@ bool isColinear(const Eigen::Vector3d& pos1, } //============================================================================== -void convertOption(const Option& fclOption, fcl::CollisionRequest& request) +void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& request) { request.num_max_contacts = fclOption.maxNumContacts; request.enable_contact = fclOption.enableContact; diff --git a/dart/collision/fcl/FCLCollisionDetector.h b/dart/collision/fcl/FCLCollisionDetector.h index adbc8294f6056..4b037a13d7a89 100644 --- a/dart/collision/fcl/FCLCollisionDetector.h +++ b/dart/collision/fcl/FCLCollisionDetector.h @@ -99,11 +99,11 @@ class FCLCollisionDetector : public CollisionDetector // Documentation inherited bool collide(CollisionGroup* group, - const Option& option, Result& result) override; + const CollisionOption& option, CollisionResult& result) override; // Documentation inherited bool collide(CollisionGroup* group1, CollisionGroup* group2, - const Option& option, Result& result) override; + const CollisionOption& option, CollisionResult& result) override; /// Set primitive shape type void setPrimitiveShapeType(PrimitiveShape type); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index c523c586b6cc0..6d2aaa5624d49 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -68,7 +68,7 @@ ConstraintSolver::ConstraintSolver(double timeStep) : mCollisionDetector(collision::FCLCollisionDetector::create()), mCollisionGroup(mCollisionDetector->createCollisionGroup()), mCollisionOption( - collision::Option( + collision::CollisionOption( true, false, 100, std::make_shared())), mTimeStep(timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) @@ -262,13 +262,13 @@ const collision::CollisionGroup* ConstraintSolver::getCollisionGroup() const } //============================================================================== -collision::Result& ConstraintSolver::getLastCollisionResult() +collision::CollisionResult& ConstraintSolver::getLastCollisionResult() { return mCollisionResult; } //============================================================================== -const collision::Result& ConstraintSolver::getLastCollisionResult() const +const collision::CollisionResult& ConstraintSolver::getLastCollisionResult() const { return mCollisionResult; } diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index da63178eb9676..135196b93a749 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -128,10 +128,10 @@ class ConstraintSolver const collision::CollisionGroup* getCollisionGroup() const; /// Return the last collision checking result - collision::Result& getLastCollisionResult(); + collision::CollisionResult& getLastCollisionResult(); /// Return the last collision checking result - const collision::Result& getLastCollisionResult() const; + const collision::CollisionResult& getLastCollisionResult() const; /// Set LCP solver void setLCPSolver(std::unique_ptr _lcpSolver); @@ -176,10 +176,10 @@ class ConstraintSolver std::unique_ptr mCollisionGroup; /// Last collision checking result - collision::Option mCollisionOption; + collision::CollisionOption mCollisionOption; /// Last collision checking result - collision::Result mCollisionResult; + collision::CollisionResult mCollisionResult; /// Time step double mTimeStep; diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 28fc1a5c8e7ff..df49a363091de 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -491,13 +491,13 @@ std::set World::removeAllSimpleFrames() //============================================================================== bool World::checkCollision(bool checkAllCollisions) { - collision::Option option; + collision::CollisionOption option; if (checkAllCollisions) option.enableContact = true; else option.enableContact = false; - collision::Result result; + collision::CollisionResult result; return mConstraintSolver->getCollisionGroup()->collide(option, result); } diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index c2620b80aa43d..d8e43562fca58 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -217,8 +217,8 @@ class MyWindow : public dart::gui::SimWindow auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); auto newGroup = collisionEngine->createCollisionGroup(object.get()); - dart::collision::Option option; - dart::collision::Result result; + dart::collision::CollisionOption option; + dart::collision::CollisionResult result; bool collision = collisionGroup->collide(newGroup.get(), option, result); // If the new object is not in collision diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index c9ba7c0980ee0..9803c6d9b3df7 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -268,8 +268,8 @@ class MyWindow : public dart::gui::SimWindow auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup(); auto newGroup = collisionEngine->createCollisionGroup(newDomino.get()); - dart::collision::Option option; - dart::collision::Result result; + dart::collision::CollisionOption option; + dart::collision::CollisionResult result; bool dominoCollision = collisionGroup->collide(newGroup.get(), option, result); diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 67ba11f17f84c..ade51d056e526 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -541,8 +541,8 @@ void testSimpleFrames(const std::shared_ptr& cd) + group2->getNumShapeFrames() + group3->getNumShapeFrames()); - collision::Option option; - collision::Result result; + collision::CollisionOption option; + collision::CollisionResult result; simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); @@ -633,8 +633,8 @@ void testBoxBox(const std::shared_ptr& cd, group1->getNumShapeFrames() + group2->getNumShapeFrames()); - collision::Option option; - collision::Result result; + collision::CollisionOption option; + collision::CollisionResult result; EXPECT_TRUE(group1->collide(group2.get(), option, result)); @@ -716,8 +716,8 @@ void testOptions(const std::shared_ptr& cd) auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get()); EXPECT_EQ(group->getNumShapeFrames(), 2u); - collision::Option option; - collision::Result result; + collision::CollisionOption option; + collision::CollisionResult result; result.clear(); option.maxNumContacts = 1000u; @@ -787,8 +787,8 @@ void testCreateCollisionGroups(const std::shared_ptr& cd) auto boxShapeNode1 = boxBodyNode1->getShapeNodesWith()[0]; auto boxShapeNode2 = boxBodyNode2->getShapeNodesWith()[0]; - collision::Option option; - collision::Result result; + collision::CollisionOption option; + collision::CollisionResult result; auto skeletonGroup1 = cd->createCollisionGroup(boxSkeleton1.get()); auto skeletonGroup2 = cd->createCollisionGroup(boxSkeleton2.get()); From 67369481c6e54e73419a55a80a04483ac7ea0a5e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 11 Apr 2016 14:25:36 -0400 Subject: [PATCH 63/67] fixed typo: destorying -> destroying --- dart/collision/CollisionDetector.cpp | 6 +++--- dart/collision/CollisionDetector.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 4a64c0b5f06a5..c78a5bdce44cb 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -66,7 +66,7 @@ std::shared_ptr CollisionDetector::claimCollisionObject( } //============================================================================== -void CollisionDetector::notifyCollisionObjectDestorying( +void CollisionDetector::notifyCollisionObjectDestroying( CollisionObject* /*object*/) { // Do nothing @@ -116,7 +116,7 @@ void CollisionDetector::NoneSharingCollisionObjectManager ::CollisionObjectDeleter::operator()(CollisionObject* object) const { - mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestorying( + mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestroying( object); delete object; @@ -178,7 +178,7 @@ void CollisionDetector::SharingCollisionObjectManager ::CollisionObjectDeleter::operator()(CollisionObject* object) const { - mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestorying( + mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestroying( object); mCollisionObjectManager->mCollisionObjectMap.erase(object->getShapeFrame()); diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 96f3bdfc72242..fb82fb5ea9ec3 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -121,7 +121,7 @@ class CollisionDetector : public std::enable_shared_from_this const dynamics::ShapeFrame* shapeFrame) = 0; /// Notify that a CollisionObject is destroying. Do nothing by default. - virtual void notifyCollisionObjectDestorying(CollisionObject* object); + virtual void notifyCollisionObjectDestroying(CollisionObject* object); protected: From 12ecc2f019045a5314cb2b61dab9d629a3e59c4c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 11 Apr 2016 16:09:32 -0400 Subject: [PATCH 64/67] isAdjacentBodies --> areAdjacentBodies --- dart/collision/CollisionFilter.cpp | 4 ++-- dart/collision/CollisionFilter.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp index ccff47cfa8baf..b611831fa5d81 100644 --- a/dart/collision/CollisionFilter.cpp +++ b/dart/collision/CollisionFilter.cpp @@ -72,7 +72,7 @@ bool BodyNodeCollisionFilter::needCollision( if (!skeleton->isEnabledAdjacentBodyCheck()) { - if (isAdjacentBodies(bodyNode1, bodyNode2)) + if (areAdjacentBodies(bodyNode1, bodyNode2)) return false; } } @@ -81,7 +81,7 @@ bool BodyNodeCollisionFilter::needCollision( } //============================================================================== -bool BodyNodeCollisionFilter::isAdjacentBodies( +bool BodyNodeCollisionFilter::areAdjacentBodies( const dynamics::BodyNode* bodyNode1, const dynamics::BodyNode* bodyNode2) const { diff --git a/dart/collision/CollisionFilter.h b/dart/collision/CollisionFilter.h index 32d7d87f6c19f..0f37401d44931 100644 --- a/dart/collision/CollisionFilter.h +++ b/dart/collision/CollisionFilter.h @@ -58,8 +58,8 @@ struct BodyNodeCollisionFilter : CollisionFilter bool needCollision(const CollisionObject* object1, const CollisionObject* object2) const override; - bool isAdjacentBodies(const dynamics::BodyNode* bodyNode1, - const dynamics::BodyNode* bodyNode2) const; + bool areAdjacentBodies(const dynamics::BodyNode* bodyNode1, + const dynamics::BodyNode* bodyNode2) const; }; } // namespace collision From 8ec2c2e1e171d870736a4406ef1a7a820588f06c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 12 Apr 2016 11:25:44 -0400 Subject: [PATCH 65/67] Rename: [None]SharingCollisionObjectManager -> ManagerFor[Nons/S]harableCollisionObjects --- dart/collision/CollisionDetector.cpp | 24 ++++++------- dart/collision/CollisionDetector.h | 35 +++++++++++-------- .../bullet/BulletCollisionDetector.cpp | 2 +- dart/collision/dart/DARTCollisionDetector.cpp | 2 +- dart/collision/fcl/FCLCollisionDetector.cpp | 2 +- 5 files changed, 35 insertions(+), 30 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index c78a5bdce44cb..c2444a4bf05cf 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -60,7 +60,7 @@ std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) { if (!mCollisionObjectManager) - mCollisionObjectManager.reset(new NoneSharingCollisionObjectManager(this)); + mCollisionObjectManager.reset(new ManagerForUnsharableCollisionObjects(this)); return mCollisionObjectManager->claimCollisionObject(shapeFrame); } @@ -82,7 +82,7 @@ CollisionDetector::CollisionObjectManager::CollisionObjectManager( //============================================================================== CollisionDetector:: -NoneSharingCollisionObjectManager::NoneSharingCollisionObjectManager( +ManagerForUnsharableCollisionObjects::ManagerForUnsharableCollisionObjects( CollisionDetector* cd) : CollisionDetector::CollisionObjectManager(cd), mCollisionObjectDeleter(this) @@ -92,7 +92,7 @@ NoneSharingCollisionObjectManager::NoneSharingCollisionObjectManager( //============================================================================== std::shared_ptr -CollisionDetector::NoneSharingCollisionObjectManager::claimCollisionObject( +CollisionDetector::ManagerForUnsharableCollisionObjects::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) { auto uniqueObject = mCollisionDetector->createCollisionObject(shapeFrame); @@ -103,9 +103,9 @@ CollisionDetector::NoneSharingCollisionObjectManager::claimCollisionObject( } //============================================================================== -CollisionDetector::NoneSharingCollisionObjectManager:: +CollisionDetector::ManagerForUnsharableCollisionObjects:: CollisionObjectDeleter::CollisionObjectDeleter( - NoneSharingCollisionObjectManager* mgr) + ManagerForUnsharableCollisionObjects* mgr) : mCollisionObjectManager(mgr) { assert(mgr); @@ -113,7 +113,7 @@ CollisionObjectDeleter::CollisionObjectDeleter( //============================================================================== void -CollisionDetector::NoneSharingCollisionObjectManager +CollisionDetector::ManagerForUnsharableCollisionObjects ::CollisionObjectDeleter::operator()(CollisionObject* object) const { mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestroying( @@ -124,7 +124,7 @@ ::CollisionObjectDeleter::operator()(CollisionObject* object) const //============================================================================== CollisionDetector:: -SharingCollisionObjectManager::SharingCollisionObjectManager( +ManagerForSharableCollisionObjects::ManagerForSharableCollisionObjects( CollisionDetector* cd) : CollisionDetector::CollisionObjectManager(cd), mCollisionObjectDeleter(this) @@ -134,14 +134,14 @@ SharingCollisionObjectManager::SharingCollisionObjectManager( //============================================================================== CollisionDetector:: -SharingCollisionObjectManager::~SharingCollisionObjectManager() +ManagerForSharableCollisionObjects::~ManagerForSharableCollisionObjects() { assert(mCollisionObjectMap.empty()); } //============================================================================== std::shared_ptr -CollisionDetector::SharingCollisionObjectManager::claimCollisionObject( +CollisionDetector::ManagerForSharableCollisionObjects::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) { const auto search = mCollisionObjectMap.find(shapeFrame); @@ -165,9 +165,9 @@ CollisionDetector::SharingCollisionObjectManager::claimCollisionObject( } //============================================================================== -CollisionDetector::SharingCollisionObjectManager:: +CollisionDetector::ManagerForSharableCollisionObjects:: CollisionObjectDeleter::CollisionObjectDeleter( - SharingCollisionObjectManager* mgr) + ManagerForSharableCollisionObjects* mgr) : mCollisionObjectManager(mgr) { assert(mgr); @@ -175,7 +175,7 @@ CollisionObjectDeleter::CollisionObjectDeleter( //============================================================================== void -CollisionDetector::SharingCollisionObjectManager +CollisionDetector::ManagerForSharableCollisionObjects ::CollisionObjectDeleter::operator()(CollisionObject* object) const { mCollisionObjectManager->mCollisionDetector->notifyCollisionObjectDestroying( diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index fb82fb5ea9ec3..c794e5276cc9c 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -95,18 +95,20 @@ class CollisionDetector : public std::enable_shared_from_this const Args&... args); /// Perform collision detection for group. - virtual bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) = 0; + virtual bool collide( + CollisionGroup* group, + const CollisionOption& option, CollisionResult& result) = 0; /// Perform collision detection for group1-group2. - virtual bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) = 0; + virtual bool collide( + CollisionGroup* group1, CollisionGroup* group2, + const CollisionOption& option, CollisionResult& result) = 0; protected: class CollisionObjectManager; - class NoneSharingCollisionObjectManager; - class SharingCollisionObjectManager; + class ManagerForUnsharableCollisionObjects; + class ManagerForSharableCollisionObjects; /// Constructor CollisionDetector() = default; @@ -129,6 +131,7 @@ class CollisionDetector : public std::enable_shared_from_this }; +//============================================================================== class CollisionDetector::CollisionObjectManager { public: @@ -147,13 +150,14 @@ class CollisionDetector::CollisionObjectManager }; -class CollisionDetector::NoneSharingCollisionObjectManager final : +//============================================================================== +class CollisionDetector::ManagerForUnsharableCollisionObjects final : public CollisionDetector::CollisionObjectManager { public: /// Constructor - NoneSharingCollisionObjectManager(CollisionDetector* cd); + ManagerForUnsharableCollisionObjects(CollisionDetector* cd); // Documentation inherited std::shared_ptr claimCollisionObject( @@ -165,9 +169,9 @@ class CollisionDetector::NoneSharingCollisionObjectManager final : /// from mCollisionObjectMap when it is not shared by any CollisionGroups. struct CollisionObjectDeleter final { - NoneSharingCollisionObjectManager* mCollisionObjectManager; + ManagerForUnsharableCollisionObjects* mCollisionObjectManager; - CollisionObjectDeleter(NoneSharingCollisionObjectManager* mgr); + CollisionObjectDeleter(ManagerForUnsharableCollisionObjects* mgr); void operator()(CollisionObject* object) const; }; @@ -176,16 +180,17 @@ class CollisionDetector::NoneSharingCollisionObjectManager final : }; -class CollisionDetector::SharingCollisionObjectManager final : +//============================================================================== +class CollisionDetector::ManagerForSharableCollisionObjects final : public CollisionDetector::CollisionObjectManager { public: /// Constructor - SharingCollisionObjectManager(CollisionDetector* cd); + ManagerForSharableCollisionObjects(CollisionDetector* cd); /// Destructor - virtual ~SharingCollisionObjectManager(); + virtual ~ManagerForSharableCollisionObjects(); // Documentation inherited std::shared_ptr claimCollisionObject( @@ -197,9 +202,9 @@ class CollisionDetector::SharingCollisionObjectManager final : /// from mCollisionObjectMap when it is not shared by any CollisionGroups. struct CollisionObjectDeleter final { - SharingCollisionObjectManager* mCollisionObjectManager; + ManagerForSharableCollisionObjects* mCollisionObjectManager; - CollisionObjectDeleter(SharingCollisionObjectManager* mgr); + CollisionObjectDeleter(ManagerForSharableCollisionObjects* mgr); void operator()(CollisionObject* object) const; }; diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index ddd7e2582ca9e..11fa0bca26d75 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -240,7 +240,7 @@ bool BulletCollisionDetector::collide( BulletCollisionDetector::BulletCollisionDetector() : CollisionDetector() { - mCollisionObjectManager.reset(new NoneSharingCollisionObjectManager(this)); + mCollisionObjectManager.reset(new ManagerForUnsharableCollisionObjects(this)); } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index a5c139e171870..ba4d3a9d599d8 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -202,7 +202,7 @@ bool DARTCollisionDetector::collide( DARTCollisionDetector::DARTCollisionDetector() : CollisionDetector() { - mCollisionObjectManager.reset(new SharingCollisionObjectManager(this)); + mCollisionObjectManager.reset(new ManagerForSharableCollisionObjects(this)); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 3fff891bae148..2181dd4be1dad 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -723,7 +723,7 @@ FCLCollisionDetector::FCLCollisionDetector() mPrimitiveShapeType(MESH), mContactPointComputationMethod(DART) { - mCollisionObjectManager.reset(new SharingCollisionObjectManager(this)); + mCollisionObjectManager.reset(new ManagerForSharableCollisionObjects(this)); } //============================================================================== From 76b866323eeecc95d0d2426f96045eaae24b5ab0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 12 Apr 2016 14:28:56 -0400 Subject: [PATCH 66/67] Remove declaration of collision detector classes from CollisionGroup --- dart/collision/CollisionGroup.h | 6 ++---- dart/collision/bullet/BulletCollisionDetector.cpp | 4 ++-- dart/collision/bullet/BulletCollisionGroup.h | 2 ++ dart/collision/fcl/FCLCollisionDetector.cpp | 11 +++++------ dart/collision/fcl/FCLCollisionGroup.h | 2 ++ 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index a9f272b2cf5d7..a1aa234b20048 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -51,9 +51,6 @@ class CollisionGroup { public: - friend class FCLCollisionDetector; - friend class BulletCollisionDetector; - /// Constructor CollisionGroup(const CollisionDetectorPtr& collisionDetector); // CollisionGroup also can be created from CollisionDetector::create() @@ -178,7 +175,8 @@ class CollisionGroup /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool collide(CollisionGroup* group, const CollisionOption& option, CollisionResult& result); + bool collide(CollisionGroup* group, + const CollisionOption& option, CollisionResult& result); protected: diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 11fa0bca26d75..68a536b8363c2 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -188,9 +188,9 @@ bool BulletCollisionDetector::collide( return false; } - group->updateEngineData(); - auto castedData = static_cast(group); + castedData->updateEngineData(); + auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); diff --git a/dart/collision/bullet/BulletCollisionGroup.h b/dart/collision/bullet/BulletCollisionGroup.h index 2cfccceaf6057..f8d8e018aeedc 100644 --- a/dart/collision/bullet/BulletCollisionGroup.h +++ b/dart/collision/bullet/BulletCollisionGroup.h @@ -85,6 +85,8 @@ class BulletCollisionGroup : public CollisionGroup protected: + using CollisionGroup::updateEngineData; + /// Bullet broad-phase algorithm std::unique_ptr mBulletProadphaseAlg; diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 2181dd4be1dad..7da9d7a7a803d 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -621,10 +621,10 @@ bool FCLCollisionDetector::collide( return false; } - group->updateEngineData(); + auto casted = static_cast(group); + casted->updateEngineData(); - auto castedData = static_cast(group); - auto broadPhaseAlg = castedData->getFCLCollisionManager(); + auto broadPhaseAlg = casted->getFCLCollisionManager(); FCLCollisionCallbackData collData( option, result, mPrimitiveShapeType, @@ -651,11 +651,10 @@ bool FCLCollisionDetector::collide( return false; } - group1->updateEngineData(); - group2->updateEngineData(); - auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); + casted1->updateEngineData(); + casted2->updateEngineData(); auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); diff --git a/dart/collision/fcl/FCLCollisionGroup.h b/dart/collision/fcl/FCLCollisionGroup.h index f5069690139b2..2df6f11d97730 100644 --- a/dart/collision/fcl/FCLCollisionGroup.h +++ b/dart/collision/fcl/FCLCollisionGroup.h @@ -63,6 +63,8 @@ class FCLCollisionGroup : public CollisionGroup protected: + using CollisionGroup::updateEngineData; + // Documentation inherited void initializeEngineData() override; From 6a16e8de7b594ccd0d1c9e945bda5e6abca3fd8c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 12 Apr 2016 21:04:31 -0400 Subject: [PATCH 67/67] Small grammatical and typo fixes --- dart/collision/CollisionDetector.h | 12 ++++++------ dart/collision/fcl/FCLCollisionDetector.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index c794e5276cc9c..c363dce5e0cb4 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -64,16 +64,16 @@ class CollisionDetector : public std::enable_shared_from_this /// Destructor virtual ~CollisionDetector() = default; - /// Return collision detection engine type in std::string + /// Return collision detection engine type as a std::string virtual const std::string& getType() const = 0; /// Create a collision group virtual std::unique_ptr createCollisionGroup() = 0; - /// Helper function that creates and returns CollisionGroup as shared_ptr. + /// Helper function that creates and returns CollisionGroup as a shared_ptr. /// - /// Internally, this function creates shared_ptr from unique_ptr returned from - /// createCollisionGroup() so the performance would be slighly worse than + /// Internally, this function creates a shared_ptr from unique_ptr returned + /// from createCollisionGroup() so the performance would be slighly worse than /// using std::make_unique. std::shared_ptr createCollisionGroupAsSharedPtr(); @@ -84,8 +84,8 @@ class CollisionDetector : public std::enable_shared_from_this /// CollisionGroup, BodyNode, and Skeleton. /// /// Note that this function adds only the ShapeFrames of each object at the - /// moment of this function is called. The aftwerward changes of the objects - /// does not affect on this CollisionGroup. + /// moment that this function is called. Any later addition to or removal of + /// the ShapeFrames that are attached to these objects will NOT be noticed. template std::unique_ptr createCollisionGroup(const Args&... args); diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 7da9d7a7a803d..9948f763200d2 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -702,7 +702,7 @@ void FCLCollisionDetector::setContactPointComputationMethod( << "it's buggy (see https://github.com/flexible-collision-library/" << "fcl/issues/106) at least until 0.4.0. It's recommended to use " << "DART's implementation for the contact point computation by " - << "settting " + << "setting " << "FCLCollisionDetector::setContactPointComputationMethod(DART).\n"; }