diff --git a/dart/collision/Contact.cpp b/dart/collision/Contact.cpp index 98f4269fbc0e8..57a825822bb1b 100644 --- a/dart/collision/Contact.cpp +++ b/dart/collision/Contact.cpp @@ -3,6 +3,7 @@ * All rights reserved. * * Author(s): Jeongseok Lee + * Michael X. Grey * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -39,5 +40,21 @@ namespace dart { namespace collision { +//============================================================================== +Contact::Contact() + : point(Eigen::Vector3d::Zero()), + normal(Eigen::Vector3d::Zero()), + force(Eigen::Vector3d::Zero()), + collisionObject1(nullptr), + collisionObject2(nullptr), + penetrationDepth(0), + triID1(0), + triID2(0), + userData(nullptr) +{ + // TODO(MXG): Consider using NaN instead of zero for uninitialized quantities + // Do nothing +} + } // namespace collision } // namespace dart diff --git a/dart/collision/Contact.h b/dart/collision/Contact.h index 7d3f5a51f5c0b..5a7416377d850 100644 --- a/dart/collision/Contact.h +++ b/dart/collision/Contact.h @@ -47,6 +47,9 @@ namespace collision { /// Contact information struct Contact { + /// Default constructor + Contact(); + /// Contact point w.r.t. the world frame Eigen::Vector3d point; diff --git a/dart/collision/Result.cpp b/dart/collision/Result.cpp index 55333262fb879..485af81942a8f 100644 --- a/dart/collision/Result.cpp +++ b/dart/collision/Result.cpp @@ -35,6 +35,11 @@ */ #include "dart/collision/Result.h" +#include "dart/collision/CollisionObject.h" + +#include "dart/dynamics/ShapeFrame.h" +#include "dart/dynamics/ShapeNode.h" +#include "dart/dynamics/BodyNode.h" namespace dart { namespace collision { @@ -43,6 +48,8 @@ namespace collision { void CollisionResult::addContact(const Contact& contact) { mContacts.push_back(contact); + addObject(contact.collisionObject1); + addObject(contact.collisionObject2); } //============================================================================== @@ -73,16 +80,74 @@ const std::vector& CollisionResult::getContacts() const return mContacts; } +//============================================================================== +const std::unordered_set +CollisionResult::getCollidingBodyNodes() const +{ + return mCollidingBodyNodes; +} + +//============================================================================== +const std::unordered_set +CollisionResult::getCollidingShapeFrames() const +{ + return mCollidingShapeFrames; +} + +//============================================================================== +bool CollisionResult::inCollision(const dynamics::BodyNode* bn) const +{ + return (mCollidingBodyNodes.find(bn) != mCollidingBodyNodes.end()); +} + +//============================================================================== +bool CollisionResult::inCollision(const dynamics::ShapeFrame* frame) const +{ + return (mCollidingShapeFrames.find(frame) != mCollidingShapeFrames.end()); +} + //============================================================================== bool CollisionResult::isCollision() const { return !mContacts.empty(); } +//============================================================================== +CollisionResult::operator bool() const +{ + return isCollision(); +} + //============================================================================== void CollisionResult::clear() { mContacts.clear(); + mCollidingShapeFrames.clear(); + mCollidingBodyNodes.clear(); +} + +//============================================================================== +void CollisionResult::addObject(CollisionObject* object) +{ + if(!object) + { + dterr << "[CollisionResult::addObject] Attempting to add a collision with " + << "a nullptr object to a CollisionResult instance. This is not " + << "allowed. Please report this as a bug!"; + assert(false); + return; + } + + const dynamics::ShapeFrame* frame = object->getShapeFrame(); + mCollidingShapeFrames.insert(frame); + + if(frame->isShapeNode()) + { + const dynamics::ShapeNode* node = + dynamic_cast(frame); + + mCollidingBodyNodes.insert(node->getBodyNodePtr()); + } } } // namespace collision diff --git a/dart/collision/Result.h b/dart/collision/Result.h index 4e3ded8970193..053a05a117ec3 100644 --- a/dart/collision/Result.h +++ b/dart/collision/Result.h @@ -38,9 +38,18 @@ #define DART_COLLISION_RESULT_H_ #include +#include #include "dart/collision/Contact.h" namespace dart { + +namespace dynamics { + +class BodyNode; +class ShapeFrame; + +} // namespace dynamics + namespace collision { class CollisionResult @@ -62,17 +71,42 @@ class CollisionResult /// Return contacts const std::vector& getContacts() const; + /// Return the set of BodyNodes that are in collision + const std::unordered_set + getCollidingBodyNodes() const; + + /// Return the set of ShapeFrames that are in collision + const std::unordered_set + getCollidingShapeFrames() const; + + /// Returns true if the given BodyNode is in collision + bool inCollision(const dynamics::BodyNode* bn) const; + + /// Returns true if the given ShapeFrame is in collision + bool inCollision(const dynamics::ShapeFrame* frame) const; + /// Return binary collision result bool isCollision() const; + /// Implicitly converts this CollisionResult to the value of isCollision() + operator bool() const; + /// Clear all the contacts void clear(); protected: + void addObject(CollisionObject* object); + /// List of contact information for each contact std::vector mContacts; + /// Set of BodyNodes that are colliding + std::unordered_set mCollidingBodyNodes; + + /// Set of ShapeFrames that are colliding + std::unordered_set mCollidingShapeFrames; + }; } // namespace collision diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 9e7da65946a12..d9bc7ae8fae5c 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -35,6 +35,7 @@ */ #include "dart/collision/dart/DARTCollide.h" +#include "dart/collision/CollisionObject.h" #include @@ -417,7 +418,8 @@ void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, // `contact' and `skip' are the contact array information provided to the // collision functions. this function only fills in the position and depth // fields. -int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, +int dBoxBox(CollisionObject* o1, CollisionObject* o2, + const dVector3 p1, const dMatrix3 R1, const dVector3 side1, const dVector3 p2, const dMatrix3 R2, const dVector3 side2, CollisionResult& result) { @@ -583,6 +585,8 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, penetration = -s; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point_vec; contact.normal = normal; contact.penetrationDepth = penetration; @@ -764,6 +768,8 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, point_vec << point[j*3+0] + pa[0], point[j*3+1] + pa[1], point[j*3+2] + pa[2]; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point_vec; contact.normal = normal; contact.penetrationDepth = dep[j]; @@ -791,6 +797,8 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, point_vec << point[iret[j]*3+0] + pa[0], point[iret[j]*3+1] + pa[1], point[iret[j]*3+2] + pa[2]; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point_vec; contact.normal = normal; contact.penetrationDepth = dep[iret[j]]; @@ -800,7 +808,8 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, return cnum; } -int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, +int collideBoxBox(CollisionObject* o1, CollisionObject* o2, + const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, CollisionResult& result) { @@ -821,10 +830,11 @@ 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(o1, o2, p1, R1, halfSize1, p0, R0, halfSize0, result); } -int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, +int collideBoxSphere(CollisionObject* o1, CollisionObject* o2, + const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const double& r1, const Eigen::Isometry3d& T1, CollisionResult& result) { @@ -874,6 +884,8 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, penetration = min + r1; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = c0; contact.normal = normal; contact.penetrationDepth = penetration; @@ -897,6 +909,8 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, normal *= (1.0/mag); Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; @@ -925,6 +939,8 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, normal = T0.linear() * normal; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; @@ -933,7 +949,8 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, return 1; } -int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, +int collideSphereBox(CollisionObject* o1, CollisionObject* o2, + const double& r0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, CollisionResult& result) { @@ -981,6 +998,8 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, penetration = min + r0; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = c0; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1004,10 +1023,13 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, normal *= (1.0/mag); Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; - result.addContact(contact);} + result.addContact(contact); + } else { double min = size[0] - std::abs(p[0]); @@ -1030,6 +1052,8 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, normal = T1.linear() * normal; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = contactpt; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1038,7 +1062,7 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, return 1; } -int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, +int collideSphereSphere(CollisionObject* o1, CollisionObject* o2, const double& _r0, const Eigen::Isometry3d& c0, const double& _r1, const Eigen::Isometry3d& c1, CollisionResult& result) { @@ -1065,6 +1089,8 @@ int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, penetration = rsum; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1077,6 +1103,8 @@ int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, penetration = rsum - normal_sqr; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1085,7 +1113,7 @@ int collideSphereSphere(const double& _r0, const Eigen::Isometry3d& c0, } -int collideCylinderSphere(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, +int collideCylinderSphere(CollisionObject* o1, CollisionObject* o2, const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const double& sphere_rad, const Eigen::Isometry3d& T1, CollisionResult& result) { @@ -1096,6 +1124,8 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons if ( dist < cyl_rad && std::abs(center[2]) < half_height + sphere_rad ) { Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; 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])); @@ -1120,6 +1150,8 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons if (penetration > 0.0) { Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1136,6 +1168,8 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons point = T0 * point; Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1147,7 +1181,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons return 0; } -int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, +int collideCylinderPlane(CollisionObject* o1, CollisionObject* o2, const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1, CollisionResult& result) { @@ -1206,6 +1240,8 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const if (penetration > 0.0) { Contact contact; + contact.collisionObject1 = o1; + contact.collisionObject2 = o2; contact.point = point; contact.normal = normal; contact.penetrationDepth = penetration; @@ -1217,10 +1253,13 @@ 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, - CollisionResult& result) +int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result) { + const dynamics::ConstShapePtr& shape0 = o1->getShape(); + const dynamics::ConstShapePtr& shape1 = o2->getShape(); + const Eigen::Isometry3d& T0 = o1->getTransform(); + const Eigen::Isometry3d& T1 = o2->getTransform(); + dynamics::Shape::ShapeType LeftType = shape0->getShapeType(); dynamics::Shape::ShapeType RightType = shape1->getShapeType(); @@ -1235,13 +1274,15 @@ int collide(dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, case dynamics::Shape::BOX: { const dynamics::BoxShape* box1 = static_cast(shape1.get()); - return collideBoxBox(box0->getSize(), T0, + return collideBoxBox(o1, o2, + box0->getSize(), T0, box1->getSize(), T1, result); } case dynamics::Shape::ELLIPSOID: { const dynamics::EllipsoidShape* ellipsoid1 = static_cast(shape1.get()); - return collideBoxSphere(box0->getSize(), T0, + return collideBoxSphere(o1, o2, + box0->getSize(), T0, ellipsoid1->getSize()[0] * 0.5, T1, result); } @@ -1264,14 +1305,16 @@ int collide(dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, case dynamics::Shape::BOX: { const dynamics::BoxShape* box1 = static_cast(shape1.get()); - return collideSphereBox(ellipsoid0->getSize()[0] * 0.5, T0, + return collideSphereBox(o1, o2, + 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, + return collideSphereSphere(o1, o2, + ellipsoid0->getSize()[0] * 0.5, T0, ellipsoid1->getSize()[0] * 0.5, T1, result); } @@ -1288,6 +1331,8 @@ int collide(dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, break; } + + return false; } } // namespace collision diff --git a/dart/collision/dart/DARTCollide.h b/dart/collision/dart/DARTCollide.h index 4a676c0c168a2..06ce1fb279cec 100644 --- a/dart/collision/dart/DARTCollide.h +++ b/dart/collision/dart/DARTCollide.h @@ -53,33 +53,38 @@ class Shape; namespace dart { namespace collision { -int collide(dart::dynamics::ConstShapePtr shape0, const Eigen::Isometry3d& T0, - dart::dynamics::ConstShapePtr shape1, const Eigen::Isometry3d& T1, +int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result); -int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, +int collideBoxBox(CollisionObject* o1, CollisionObject* o2, + const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, CollisionResult& result); -int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, +int collideBoxSphere(CollisionObject* o1, CollisionObject* o2, + const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, const double& r1, const Eigen::Isometry3d& T1, CollisionResult& result); -int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, +int collideSphereBox(CollisionObject* o1, CollisionObject* o2, + const double& r0, const Eigen::Isometry3d& T0, const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1, CollisionResult& result); -int collideSphereSphere(const double& r0, const Eigen::Isometry3d& c0, +int collideSphereSphere(CollisionObject* o1, CollisionObject* o2, + const double& r0, const Eigen::Isometry3d& c0, const double& r1, const Eigen::Isometry3d& c1, CollisionResult& result); int collideCylinderSphere( + CollisionObject* o1, CollisionObject* o2, const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const double& sphere_rad, const Eigen::Isometry3d& T1, CollisionResult& result); int collideCylinderPlane( + CollisionObject* o1, CollisionObject* o2, const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0, const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1, diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index ba4d3a9d599d8..4bdcc6740bba6 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -254,9 +254,7 @@ bool checkPair(CollisionObject* o1, CollisionObject* o2, CollisionResult pairResult; // Perform narrow-phase detection - auto colliding = collide(o1->getShape(), o1->getTransform(), - o2->getShape(), o2->getTransform(), - pairResult); + auto colliding = collide(o1, o2, pairResult); postProcess(o1, o2, option, result, pairResult); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index f5507da750220..97179a22902c8 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -711,10 +711,12 @@ class BodyNode : /// this status is set by the constraint solver during dynamics simulation but /// not by collision detector. /// \param[in] True if this body node is colliding. + DEPRECATED(6.0) void setColliding(bool _isColliding); /// Return whether this body node is set to be colliding with other objects. /// \return True if this body node is colliding. + DEPRECATED(6.0) bool isColliding(); /// Add applying linear Cartesian forces to this node @@ -1033,6 +1035,7 @@ class BodyNode : UniqueProperties mBodyP; /// Whether the node is currently in collision with another node. + DEPRECATED(6.0) bool mIsColliding; //-------------------------------------------------------------------------- diff --git a/dart/dynamics/MetaSkeleton.h b/dart/dynamics/MetaSkeleton.h index 0b8c816fcc4d6..53e4aa36fce5c 100644 --- a/dart/dynamics/MetaSkeleton.h +++ b/dart/dynamics/MetaSkeleton.h @@ -538,6 +538,7 @@ class MetaSkeleton : public common::Subject virtual double getPotentialEnergy() const = 0; /// Clear collision flags of the BodyNodes in this MetaSkeleton + DEPRECATED(6.0) virtual void clearCollidingBodies() = 0; /// \} diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h index d7a44d24ed891..fc69bad483f07 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -274,6 +274,7 @@ class ReferentialSkeleton : public MetaSkeleton double getPotentialEnergy() const override; // Documentation inherited + DEPRECATED(6.0) void clearCollidingBodies() override; /// \} diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 2bf283e515019..c65a5431f0512 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -879,6 +879,7 @@ class Skeleton : double getPotentialEnergy() const override; // Documentation inherited + DEPRECATED(6.0) void clearCollidingBodies() override; /// \}