From 46e9c03516f9bc71d60c7e066b0e02d82cf66277 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 14 Apr 2016 15:17:29 -0400 Subject: [PATCH] Let ConstraintSolver set the colliding status of PointMass --- dart/constraint/ConstraintSolver.cpp | 9 +++++++-- dart/constraint/SoftContactConstraint.cpp | 2 ++ dart/dynamics/BodyNode.cpp | 12 ++++++++++++ dart/dynamics/BodyNode.h | 15 ++++++++++++--- dart/dynamics/PointMass.h | 6 ++++-- dart/dynamics/ReferentialSkeleton.cpp | 17 +++++++++++++---- dart/dynamics/Skeleton.cpp | 16 ++++++++++++---- dart/dynamics/SoftBodyNode.cpp | 12 ++++++++++++ dart/dynamics/SoftBodyNode.h | 6 ++++++ 9 files changed, 80 insertions(+), 15 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 07a59484a128d..c4dc820c89356 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -398,8 +398,13 @@ void ConstraintSolver::updateConstraints() auto& ct = mCollisionResult.getContact(i); // Set colliding bodies - const_cast(ct.collisionObject1->getShapeFrame())->asShapeNode()->getBodyNodePtr()->setColliding(true); - const_cast(ct.collisionObject2->getShapeFrame())->asShapeNode()->getBodyNodePtr()->setColliding(true); + auto shapeFrame1 = const_cast( + ct.collisionObject1->getShapeFrame()); + auto shapeFrame2 = const_cast( + ct.collisionObject2->getShapeFrame()); + + shapeFrame1->asShapeNode()->getBodyNodePtr()->setColliding(true); + shapeFrame2->asShapeNode()->getBodyNodePtr()->setColliding(true); if (isSoftContact(ct)) { diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index 2af46a45b7618..b97d0be891984 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -109,6 +109,7 @@ SoftContactConstraint::SoftContactConstraint( { mPointMass1 = selectCollidingPointMass(mSoftBodyNode1, contact.point, contact.triID1); + mPointMass1->setColliding(true); } } if (mSoftBodyNode2) @@ -118,6 +119,7 @@ SoftContactConstraint::SoftContactConstraint( { mPointMass2 = selectCollidingPointMass(mSoftBodyNode2, contact.point, contact.triID2); + mPointMass2->setColliding(true); } } diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 26f3c245117c1..9fa3b9a3e8c66 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -180,6 +180,18 @@ BodyNode::~BodyNode() delete mParentJoint; } +//============================================================================== +SoftBodyNode* BodyNode::asSoftBodyNode() +{ + return nullptr; +} + +//============================================================================== +const SoftBodyNode* BodyNode::asSoftBodyNode() const +{ + return nullptr; +} + //============================================================================== void BodyNode::setProperties(const ExtendedProperties& _properties) { diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index d5b9a8d574939..f5507da750220 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -103,6 +103,14 @@ class BodyNode : /// Destructor virtual ~BodyNode(); + /// Convert 'this' into a SoftBodyNode pointer if this BodyNode is a + /// SoftBodyNode, otherwise return nullptr + virtual SoftBodyNode* asSoftBodyNode(); + + /// Convert 'const this' into a SoftBodyNode pointer if this BodyNode is a + /// SoftBodyNode, otherwise return nullptr + virtual const SoftBodyNode* asSoftBodyNode() const; + /// Set the ExtendedProperties of this BodyNode void setProperties(const ExtendedProperties& _properties); @@ -699,12 +707,13 @@ class BodyNode : /// Return the velocity change due to the constraint impulse const Eigen::Vector6d& getBodyVelocityChange() const; - /// Set whether this body node is colliding with others. This is called by - /// collision detector. + /// Set whether this body node is colliding with other objects. Note that + /// 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. void setColliding(bool _isColliding); - /// Return whether this body node is colliding with others + /// Return whether this body node is set to be colliding with other objects. /// \return True if this body node is colliding. bool isColliding(); diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 6df83d7be6969..95fa3de38a0e7 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -156,11 +156,13 @@ class PointMass : public common::Subject const PointMass* getConnectedPointMass(size_t _idx) const; - /// Set whether this point mass is colliding with others. + /// Set whether this point mass is colliding with other objects. Note that + /// this status is set by the constraint solver during dynamics simulation but + /// not by collision detector. /// \param[in] True if this point mass is colliding. void setColliding(bool _isColliding); - /// Get whether this point mass is colliding with others. + /// Return whether this point mass is set to be colliding with other objects. /// \return True if this point mass is colliding. bool isColliding(); diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 1de7b7eff6fd7..676fe22287370 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -36,6 +36,7 @@ #include "dart/dynamics/ReferentialSkeleton.h" #include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/DegreeOfFreedom.h" @@ -788,11 +789,19 @@ double ReferentialSkeleton::getPotentialEnergy() const //============================================================================== void ReferentialSkeleton::clearCollidingBodies() { - for(size_t i = 0; i < this->getNumBodyNodes(); i++) + for (auto i = 0u; i < getNumBodyNodes(); ++i) { - auto bd = this->getBodyNode(i); - if(bd) - bd->setColliding(false); + auto bodyNode = getBodyNode(i); + bodyNode->setColliding(false); + + auto softBodyNode = bodyNode->asSoftBodyNode(); + if (softBodyNode) + { + auto& pointMasses = softBodyNode->getPointMasses(); + + for (auto pointMass : pointMasses) + pointMass->setColliding(false); + } } } diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 243ee8518d153..6a6a6c1226e0e 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -3616,11 +3616,19 @@ double Skeleton::getPotentialEnergy() const //============================================================================== void Skeleton::clearCollidingBodies() { - for(size_t i = 0; i < this->getNumBodyNodes(); i++) + for (auto i = 0u; i < getNumBodyNodes(); ++i) { - auto bd = this->getBodyNode(i); - if(bd) - bd->setColliding(false); + auto bodyNode = getBodyNode(i); + bodyNode->setColliding(false); + + auto softBodyNode = bodyNode->asSoftBodyNode(); + if (softBodyNode) + { + auto& pointMasses = softBodyNode->getPointMasses(); + + for (auto pointMass : pointMasses) + pointMass->setColliding(false); + } } } diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 3e47a4e2f09f8..ad011c7799367 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -127,6 +127,18 @@ SoftBodyNode::~SoftBodyNode() delete mNotifier; } +//============================================================================== +SoftBodyNode* SoftBodyNode::asSoftBodyNode() +{ + return this; +} + +//============================================================================== +const SoftBodyNode* SoftBodyNode::asSoftBodyNode() const +{ + return this; +} + //============================================================================== void SoftBodyNode::setProperties(const Properties& _properties) { diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index cac06047f8b26..9be2fdc64a16d 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -121,6 +121,12 @@ class SoftBodyNode : public BodyNode /// \brief virtual ~SoftBodyNode(); + // Documentation inherited + SoftBodyNode* asSoftBodyNode() override; + + // Documentation inherited + const SoftBodyNode* asSoftBodyNode() const override; + /// Set the Properties of this SoftBodyNode void setProperties(const Properties& _properties);