diff --git a/modules/bullet/SCsub b/modules/bullet/SCsub index 468d2ad62e4..194ed2030ee 100644 --- a/modules/bullet/SCsub +++ b/modules/bullet/SCsub @@ -152,6 +152,7 @@ if env["builtin_bullet"]: "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp", "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp", "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp", + "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp", "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp", "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp", "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp", @@ -177,6 +178,10 @@ if env["builtin_bullet"]: "BulletSoftBody/btDeformableContactProjection.cpp", "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp", "BulletSoftBody/btDeformableContactConstraint.cpp", + "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp", + "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp", + "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp", + "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp", "BulletSoftBody/poly34.cpp", # clew "clew/clew.c", diff --git a/thirdparty/README.md b/thirdparty/README.md index 1d2775265c3..3401b9db9bb 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -20,7 +20,7 @@ Files extracted from upstream source: ## bullet - Upstream: https://github.com/bulletphysics/bullet3 -- Version: 3.21 (6a59241074720e9df119f2f86bc01765917feb1e, 2021) +- Version: 3.24 (7dee3436e747958e7088dfdcea0e4ae031ce619e, 2022) - License: zlib Files extracted from upstream source: diff --git a/thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h b/thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h index 7d31b56d030..8ef3331f77a 100644 --- a/thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h +++ b/thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h @@ -135,7 +135,11 @@ public: int otherSize = otherArray.size(); resize(otherSize); - otherArray.copy(0, otherSize, m_data); + //don't use otherArray.copy, it can leak memory + for (int i = 0; i < otherSize; i++) + { + m_data[i] = otherArray[i]; + } } /// return the number of elements in the array @@ -506,7 +510,11 @@ public: { int otherSize = otherArray.size(); resize(otherSize); - otherArray.copy(0, otherSize, m_data); + //don't use otherArray.copy, it can leak memory + for (int i = 0; i < otherSize; i++) + { + m_data[i] = otherArray[i]; + } } void removeAtIndex(int index) diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index c3f7f740c08..15940f5b6db 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -198,7 +198,6 @@ void btCollisionWorld::updateAabbs() { BT_PROFILE("updateAabbs"); - btTransform predictedTrans; for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 52fb6b43eeb..8d59ba95ae5 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -103,6 +103,44 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, if (m_convexBodyWrap->getCollisionShape()->isConvex()) { +#ifndef BT_DISABLE_CONVEX_CONCAVE_EARLY_OUT + //an early out optimisation if the object is separated from the triangle + //projected on the triangle normal) + { + const btVector3 v0 = m_triBodyWrap->getWorldTransform()*triangle[0]; + const btVector3 v1 = m_triBodyWrap->getWorldTransform()*triangle[1]; + const btVector3 v2 = m_triBodyWrap->getWorldTransform()*triangle[2]; + + btVector3 triangle_normal_world = ( v1 - v0).cross(v2 - v0); + triangle_normal_world.normalize(); + + btConvexShape* convex = (btConvexShape*)m_convexBodyWrap->getCollisionShape(); + + btVector3 localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world); + btVector3 worldPt = m_convexBodyWrap->getWorldTransform()*localPt; + //now check if this is fully on one side of the triangle + btScalar proj_distPt = triangle_normal_world.dot(worldPt); + btScalar proj_distTr = triangle_normal_world.dot(v0); + btScalar contact_threshold = m_manifoldPtr->getContactBreakingThreshold()+ m_resultOut->m_closestPointDistanceThreshold; + btScalar dist = proj_distTr - proj_distPt; + if (dist > contact_threshold) + return; + + //also check the other side of the triangle + triangle_normal_world*=-1; + + localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world); + worldPt = m_convexBodyWrap->getWorldTransform()*localPt; + //now check if this is fully on one side of the triangle + proj_distPt = triangle_normal_world.dot(worldPt); + proj_distTr = triangle_normal_world.dot(v0); + + dist = proj_distTr - proj_distPt; + if (dist > contact_threshold) + return; + } +#endif //BT_DISABLE_CONVEX_CONCAVE_EARLY_OUT + btTriangleShape tm(triangle[0], triangle[1], triangle[2]); tm.setMargin(m_collisionMarginTriangle); @@ -132,7 +170,10 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, m_resultOut->setShapeIdentifiersB(partId, triangleIndex); } - colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); + { + BT_PROFILE("processCollision (GJK?)"); + colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); + } if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h index 96136d7dd72..01058a6c314 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -25,6 +25,7 @@ subject to the following restrictions: ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape { +protected: btAlignedObjectArray m_unscaledPoints; public: diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index 3b5150f6d50..f5763f6988a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -43,6 +43,9 @@ public: void setTransformB(const btTransform& transB) { m_transB = transB; } const btTransform& getTransformA() const { return m_transA; } + const btTransform& getTransformB() const { return m_transB; } + + // keep this for backward compatibility const btTransform& GetTransformB() const { return m_transB; } virtual btScalar getMargin() const; diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 70915366e07..8cc373db2ec 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -71,8 +71,8 @@ public: const btVector3& normal, btScalar distance) : m_localPointA(pointA), m_localPointB(pointB), - m_positionWorldOnB(0,0,0), - m_positionWorldOnA(0,0,0), + m_positionWorldOnB(0,0,0), + m_positionWorldOnA(0,0,0), m_normalWorldOnB(normal), m_distance1(distance), m_combinedFriction(btScalar(0.)), @@ -95,8 +95,8 @@ public: m_contactERP(0.f), m_frictionCFM(0.f), m_lifeTime(0), - m_lateralFrictionDir1(0,0,0), - m_lateralFrictionDir2(0,0,0) + m_lateralFrictionDir1(0,0,0), + m_lateralFrictionDir2(0,0,0) { } diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index fb15ae31eb5..9e99c154f20 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -902,8 +902,8 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bod btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction; btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); - btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject); btMutexLock(&m_predictiveManifoldsMutex); + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject); m_predictiveManifolds.push_back(manifold); btMutexUnlock(&m_predictiveManifoldsMutex); diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 1aaa07b69e7..7287ccc89ba 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -31,7 +31,7 @@ enum btTypedMultiBodyConstraintType MULTIBODY_CONSTRAINT_SLIDER, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, MULTIBODY_CONSTRAINT_FIXED, - + MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT, MAX_MULTIBODY_CONSTRAINT_TYPE, }; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp new file mode 100644 index 00000000000..27c7520dc31 --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp @@ -0,0 +1,270 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodySphericalJointLimit.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "LinearMath/btTransformUtil.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit(btMultiBody* body, int link, + btScalar swingxRange, + btScalar swingyRange, + btScalar twistRange, + btScalar maxAppliedImpulse) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT), + m_desiredVelocity(0, 0, 0), + m_desiredPosition(0,0,0,1), + m_use_multi_dof_params(false), + m_kd(1., 1., 1.), + m_kp(0.2, 0.2, 0.2), + m_erp(1), + m_rhsClamp(SIMD_INFINITY), + m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse), + m_pivotA(m_bodyA->getLink(link).m_eVector), + m_pivotB(m_bodyB->getLink(link).m_eVector), + m_swingxRange(swingxRange), + m_swingyRange(swingyRange), + m_twistRange(twistRange) + +{ + + m_maxAppliedImpulse = maxAppliedImpulse; +} + + +void btMultiBodySphericalJointLimit::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; + + jacobianB(1)[offset] = -1; + + m_numDofsFinalized = m_jacSizeBoth; +} + + +btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit() +{ +} + +int btMultiBodySphericalJointLimit::getIslandIdA() const +{ + if (this->m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + { + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySphericalJointLimit::getIslandIdB() const +{ + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + { + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} + +void btMultiBodySphericalJointLimit::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + + if (m_maxAppliedImpulse == 0.f) + return; + + const btScalar posError = 0; + const btVector3 zero(0, 0, 0); + + + btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) }; + + btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0], + m_bodyA->getJointPosMultiDof(m_linkA)[1], + m_bodyA->getJointPosMultiDof(m_linkA)[2], + m_bodyA->getJointPosMultiDof(m_linkA)[3]); + + btQuaternion refQuat = m_desiredPosition; + btVector3 vTwist(0,0,1); + + btVector3 vConeNoTwist = quatRotate(currentQuat, vTwist); + vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); + qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * currentQuat; + qABTwist.normalize(); + btQuaternion desiredQuat = qABTwist; + + + btQuaternion relRot = currentQuat.inverse() * desiredQuat; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff); + + btScalar limitRanges[3] = {m_swingxRange, m_swingyRange, m_twistRange}; + + /// twist axis/angle + btQuaternion qMinTwist = qABTwist; + btScalar twistAngle = qABTwist.getAngle(); + + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. + { + qMinTwist = -(qABTwist); + twistAngle = qMinTwist.getAngle(); + } + btVector3 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); + if (twistAngle > SIMD_EPSILON) + vTwistAxis.normalize(); + + if (vTwistAxis.dot(vTwist)<0) + twistAngle*=-1.; + + angleDiff[2] = twistAngle; + + + for (int row = 0; row < getNumRows(); row++) + { + btScalar allowed = limitRanges[row]; + btScalar damp = 1; + if((angleDiff[row]>-allowed)&&(angleDiff[row]allowed) + { + angleDiff[row]-=allowed; + + } + if (angleDiff[row]<-allowed) + { + angleDiff[row]+=allowed; + + } + } + + + int dof = row; + + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar desiredVelocity = this->m_desiredVelocity[row]; + + double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0]; + btScalar velocityError = (desiredVelocity - currentVelocity) * kd; + + btMatrix3x3 frameAworld; + frameAworld.setIdentity(); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + btScalar posError = 0; + { + btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eSpherical: + { + btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); + double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0]; + posError = kp*angleDiff[row % 3]; + double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse; + //should multiply by time step + //max_applied_impulse *= infoGlobal.m_timeStep + + double min_applied_impulse = -max_applied_impulse; + + + if (posError>0) + max_applied_impulse=0; + else + min_applied_impulse=0; + + if (btFabs(posError)>SIMD_EPSILON) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + zero, zero, zero,//pure angular, so zero out linear parts + posError, + infoGlobal, + min_applied_impulse, max_applied_impulse, true, + 1.0, false, 0, 0, + damp); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + } + break; + } + default: + { + btAssert(0); + } + }; + } + } +} + + +void btMultiBodySphericalJointLimit::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h new file mode 100644 index 00000000000..b82db6a99e5 --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h @@ -0,0 +1,115 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H +#define BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodySphericalJointLimit : public btMultiBodyConstraint +{ +protected: + btVector3 m_desiredVelocity; + btQuaternion m_desiredPosition; + bool m_use_multi_dof_params; + btVector3 m_kd; + btVector3 m_kp; + btScalar m_erp; + btScalar m_rhsClamp; //maximum error + btVector3 m_maxAppliedImpulseMultiDof; + btVector3 m_pivotA; + btVector3 m_pivotB; + btScalar m_swingxRange; + btScalar m_swingyRange; + btScalar m_twistRange; + +public: + btMultiBodySphericalJointLimit(btMultiBody* body, int link, + btScalar swingxRange, + btScalar swingyRange, + btScalar twistRange, + btScalar maxAppliedImpulse); + + virtual ~btMultiBodySphericalJointLimit(); + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0) + { + m_desiredVelocity = velTarget; + m_kd = btVector3(kd, kd, kd); + m_use_multi_dof_params = false; + } + + virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0)) + { + m_desiredVelocity = velTarget; + m_kd = kd; + m_use_multi_dof_params = true; + } + + virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f) + { + m_desiredPosition = posTarget; + m_kp = btVector3(kp, kp, kp); + m_use_multi_dof_params = false; + } + + virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f)) + { + m_desiredPosition = posTarget; + m_kp = kp; + m_use_multi_dof_params = true; + } + + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } + virtual void setRhsClamp(btScalar rhsClamp) + { + m_rhsClamp = rhsClamp; + } + + btScalar getMaxAppliedImpulseMultiDof(int i) const + { + return m_maxAppliedImpulseMultiDof[i]; + } + + void setMaxAppliedImpulseMultiDof(const btVector3& maxImp) + { + m_maxAppliedImpulseMultiDof = maxImp; + m_use_multi_dof_params = true; + } + + + virtual void debugDraw(class btIDebugDraw* drawer); + +}; + +#endif //BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp index 2f120ed489d..0c3f5c9d474 100644 --- a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp @@ -503,8 +503,8 @@ vec3 rpyFromMatrix(const mat33 &rot) { vec3 rpy; rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0)); - rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0)); rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2)); + rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0)); return rpy; } } // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp new file mode 100644 index 00000000000..feb30d58791 --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp @@ -0,0 +1,792 @@ +#include "btReducedDeformableBody.h" +#include "../btSoftBodyInternals.h" +#include "btReducedDeformableBodyHelpers.h" +#include "LinearMath/btTransformUtil.h" +#include +#include + +btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) + : btSoftBody(worldInfo, node_count, x, m), m_rigidOnly(false) +{ + // reduced deformable + m_reducedModel = true; + m_nReduced = 0; + m_nFull = 0; + m_nodeIndexOffset = 0; + + m_transform_lock = false; + m_ksScale = 1.0; + m_rhoScale = 1.0; + + // rigid motion + m_linearVelocity.setZero(); + m_angularVelocity.setZero(); + m_internalDeltaLinearVelocity.setZero(); + m_internalDeltaAngularVelocity.setZero(); + m_angularVelocityFromReduced.setZero(); + m_internalDeltaAngularVelocityFromReduced.setZero(); + m_angularFactor.setValue(1, 1, 1); + m_linearFactor.setValue(1, 1, 1); + // m_invInertiaLocal.setValue(1, 1, 1); + m_invInertiaLocal.setIdentity(); + m_mass = 0.0; + m_inverseMass = 0.0; + + m_linearDamping = 0; + m_angularDamping = 0; + + // Rayleigh damping + m_dampingAlpha = 0; + m_dampingBeta = 0; + + m_rigidTransformWorld.setIdentity(); +} + +void btReducedDeformableBody::setReducedModes(int num_modes, int full_size) +{ + m_nReduced = num_modes; + m_nFull = full_size; + m_reducedDofs.resize(m_nReduced, 0); + m_reducedDofsBuffer.resize(m_nReduced, 0); + m_reducedVelocity.resize(m_nReduced, 0); + m_reducedVelocityBuffer.resize(m_nReduced, 0); + m_reducedForceElastic.resize(m_nReduced, 0); + m_reducedForceDamping.resize(m_nReduced, 0); + m_reducedForceExternal.resize(m_nReduced, 0); + m_internalDeltaReducedVelocity.resize(m_nReduced, 0); + m_nodalMass.resize(full_size, 0); + m_localMomentArm.resize(m_nFull); +} + +void btReducedDeformableBody::setMassProps(const tDenseArray& mass_array) +{ + btScalar total_mass = 0; + btVector3 CoM(0, 0, 0); + for (int i = 0; i < m_nFull; ++i) + { + m_nodalMass[i] = m_rhoScale * mass_array[i]; + m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0; + total_mass += m_rhoScale * mass_array[i]; + + CoM += m_nodalMass[i] * m_nodes[i].m_x; + } + // total rigid body mass + m_mass = total_mass; + m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0; + // original CoM + m_initialCoM = CoM / total_mass; +} + +void btReducedDeformableBody::setInertiaProps() +{ + // make sure the initial CoM is at the origin (0,0,0) + // for (int i = 0; i < m_nFull; ++i) + // { + // m_nodes[i].m_x -= m_initialCoM; + // } + // m_initialCoM.setZero(); + m_rigidTransformWorld.setOrigin(m_initialCoM); + m_interpolationWorldTransform = m_rigidTransformWorld; + + updateLocalInertiaTensorFromNodes(); + + // update world inertia tensor + btMatrix3x3 rotation; + rotation.setIdentity(); + updateInitialInertiaTensor(rotation); + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; +} + +void btReducedDeformableBody::setRigidVelocity(const btVector3& v) +{ + m_linearVelocity = v; +} + +void btReducedDeformableBody::setRigidAngularVelocity(const btVector3& omega) +{ + m_angularVelocity = omega; +} + +void btReducedDeformableBody::setStiffnessScale(const btScalar ks) +{ + m_ksScale = ks; +} + +void btReducedDeformableBody::setMassScale(const btScalar rho) +{ + m_rhoScale = rho; +} + +void btReducedDeformableBody::setFixedNodes(const int n_node) +{ + m_fixedNodes.push_back(n_node); + m_nodes[n_node].m_im = 0; // set inverse mass to be zero for the constraint solver. +} + +void btReducedDeformableBody::setDamping(const btScalar alpha, const btScalar beta) +{ + m_dampingAlpha = alpha; + m_dampingBeta = beta; +} + +void btReducedDeformableBody::internalInitialization() +{ + // zeroing + endOfTimeStepZeroing(); + // initialize rest position + updateRestNodalPositions(); + // initialize local nodal moment arm form the CoM + updateLocalMomentArm(); + // initialize projection matrix + updateExternalForceProjectMatrix(false); +} + +void btReducedDeformableBody::updateLocalMomentArm() +{ + TVStack delta_x; + delta_x.resize(m_nFull); + + for (int i = 0; i < m_nFull; ++i) + { + for (int k = 0; k < 3; ++k) + { + // compute displacement + delta_x[i][k] = 0; + for (int j = 0; j < m_nReduced; ++j) + { + delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j]; + } + } + // get new moment arm Sq + x0 + m_localMomentArm[i] = m_x0[i] - m_initialCoM + delta_x[i]; + } +} + +void btReducedDeformableBody::updateExternalForceProjectMatrix(bool initialized) +{ + // if not initialized, need to compute both P_A and Cq + // otherwise, only need to udpate Cq + if (!initialized) + { + // resize + m_projPA.resize(m_nReduced); + m_projCq.resize(m_nReduced); + + m_STP.resize(m_nReduced); + m_MrInvSTP.resize(m_nReduced); + + // P_A + for (int r = 0; r < m_nReduced; ++r) + { + m_projPA[r].resize(3 * m_nFull, 0); + for (int i = 0; i < m_nFull; ++i) + { + btMatrix3x3 mass_scaled_i = Diagonal(1) - Diagonal(m_nodalMass[i] / m_mass); + btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + btVector3 prod_i = mass_scaled_i * s_ri; + + for (int k = 0; k < 3; ++k) + m_projPA[r][3 * i + k] = prod_i[k]; + + // btScalar ratio = m_nodalMass[i] / m_mass; + // m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio, + // - m_modes[r][3 * i + 1] * ratio, + // - m_modes[r][3 * i + 2] * ratio); + } + } + } + + // C(q) is updated once per position update + for (int r = 0; r < m_nReduced; ++r) + { + m_projCq[r].resize(3 * m_nFull, 0); + for (int i = 0; i < m_nFull; ++i) + { + btMatrix3x3 r_star = Cross(m_localMomentArm[i]); + btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + btVector3 prod_i = r_star * m_invInertiaTensorWorld * r_star * s_ri; + + for (int k = 0; k < 3; ++k) + m_projCq[r][3 * i + k] = m_nodalMass[i] * prod_i[k]; + + // btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + // m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]); + } + } +} + +void btReducedDeformableBody::endOfTimeStepZeroing() +{ + for (int i = 0; i < m_nReduced; ++i) + { + m_reducedForceElastic[i] = 0; + m_reducedForceDamping[i] = 0; + m_reducedForceExternal[i] = 0; + m_internalDeltaReducedVelocity[i] = 0; + m_reducedDofsBuffer[i] = m_reducedDofs[i]; + m_reducedVelocityBuffer[i] = m_reducedVelocity[i]; + } + // std::cout << "zeroed!\n"; +} + +void btReducedDeformableBody::applyInternalVelocityChanges() +{ + m_linearVelocity += m_internalDeltaLinearVelocity; + m_angularVelocity += m_internalDeltaAngularVelocity; + m_internalDeltaLinearVelocity.setZero(); + m_internalDeltaAngularVelocity.setZero(); + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r]; + m_internalDeltaReducedVelocity[r] = 0; + } +} + +void btReducedDeformableBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform) +{ + btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform); +} + +void btReducedDeformableBody::updateReducedDofs(btScalar solverdt) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedDofs[r] = m_reducedDofsBuffer[r] + solverdt * m_reducedVelocity[r]; + } +} + +void btReducedDeformableBody::mapToFullPosition(const btTransform& ref_trans) +{ + btVector3 origin = ref_trans.getOrigin(); + btMatrix3x3 rotation = ref_trans.getBasis(); + + + for (int i = 0; i < m_nFull; ++i) + { + m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin; + m_nodes[i].m_q = m_nodes[i].m_x; + } +} + +void btReducedDeformableBody::updateReducedVelocity(btScalar solverdt) +{ + // update reduced velocity + for (int r = 0; r < m_nReduced; ++r) + { + // the reduced mass is always identity! + btScalar delta_v = 0; + delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r]); + // delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]); + m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v; + } +} + +void btReducedDeformableBody::mapToFullVelocity(const btTransform& ref_trans) +{ + // compute the reduced contribution to the angular velocity + // btVector3 sum_linear(0, 0, 0); + // btVector3 sum_angular(0, 0, 0); + // m_linearVelocityFromReduced.setZero(); + // m_angularVelocityFromReduced.setZero(); + // for (int i = 0; i < m_nFull; ++i) + // { + // btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i]; + // btMatrix3x3 r_star = Cross(r_com); + + // btVector3 v_from_reduced(0, 0, 0); + // for (int k = 0; k < 3; ++k) + // { + // for (int r = 0; r < m_nReduced; ++r) + // { + // v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r]; + // } + // } + + // btVector3 delta_linear = m_nodalMass[i] * v_from_reduced; + // btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced); + // sum_linear += delta_linear; + // sum_angular += delta_angular; + // // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n"; + // // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n"; + // // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n"; + // // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n"; + // } + // m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear); + // m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular; + + // m_linearVelocity -= m_linearVelocityFromReduced; + // m_angularVelocity -= m_angularVelocityFromReduced; + + for (int i = 0; i < m_nFull; ++i) + { + m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i); + } +} + +const btVector3 btReducedDeformableBody::computeTotalAngularMomentum() const +{ + btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity; + btVector3 L_reduced(0, 0, 0); + btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced); + + for (int i = 0; i < m_nFull; ++i) + { + btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i]; + btMatrix3x3 r_star = Cross(r_com); + + btVector3 v_from_reduced(0, 0, 0); + for (int k = 0; k < 3; ++k) + { + for (int r = 0; r < m_nReduced; ++r) + { + v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r]; + } + } + + L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com)); + // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced)); + } + return L_rigid + L_reduced; +} + +const btVector3 btReducedDeformableBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const +{ + btVector3 v_from_reduced(0, 0, 0); + btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node]; + // compute velocity contributed by the reduced velocity + for (int k = 0; k < 3; ++k) + { + for (int r = 0; r < m_nReduced; ++r) + { + v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r]; + } + } + // get new velocity + btVector3 vel = m_angularVelocity.cross(r_com) + + ref_trans.getBasis() * v_from_reduced + + m_linearVelocity; + return vel; +} + +const btVector3 btReducedDeformableBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const +{ + btVector3 deltaV_from_reduced(0, 0, 0); + btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node]; + + // compute velocity contributed by the reduced velocity + for (int k = 0; k < 3; ++k) + { + for (int r = 0; r < m_nReduced; ++r) + { + deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r]; + } + } + + // get delta velocity + btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) + + ref_trans.getBasis() * deltaV_from_reduced + + m_internalDeltaLinearVelocity; + return deltaV; +} + +void btReducedDeformableBody::proceedToTransform(btScalar dt, bool end_of_time_step) +{ + btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform); + updateInertiaTensor(); + // m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose(); + m_rigidTransformWorld = m_interpolationWorldTransform; + m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld; +} + +void btReducedDeformableBody::transformTo(const btTransform& trs) +{ + btTransform current_transform = getRigidTransform(); + btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(), + trs.getOrigin() - current_transform.getOrigin()); + transform(new_transform); +} + +void btReducedDeformableBody::transform(const btTransform& trs) +{ + m_transform_lock = true; + + // transform mesh + { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + btVector3 CoM = m_rigidTransformWorld.getOrigin(); + btVector3 translation = trs.getOrigin(); + btMatrix3x3 rotation = trs.getBasis(); + + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + n.m_x = rotation * (n.m_x - CoM) + CoM + translation; + n.m_q = rotation * (n.m_q - CoM) + CoM + translation; + n.m_n = rotation * n.m_n; + vol = btDbvtVolume::FromCR(n.m_x, margin); + + m_ndbvt.update(n.m_leaf, vol); + } + updateNormals(); + updateBounds(); + updateConstants(); + } + + // update modes + updateModesByRotation(trs.getBasis()); + + // update inertia tensor + updateInitialInertiaTensor(trs.getBasis()); + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; + + // update rigid frame (No need to update the rotation. Nodes have already been updated.) + m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin()); + m_interpolationWorldTransform = m_rigidTransformWorld; + m_initialCoM = m_rigidTransformWorld.getOrigin(); + + internalInitialization(); +} + +void btReducedDeformableBody::scale(const btVector3& scl) +{ + // Scaling the mesh after transform is applied is not allowed + btAssert(!m_transform_lock); + + // scale the mesh + { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + btVector3 CoM = m_rigidTransformWorld.getOrigin(); + + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + n.m_x = (n.m_x - CoM) * scl + CoM; + n.m_q = (n.m_q - CoM) * scl + CoM; + vol = btDbvtVolume::FromCR(n.m_x, margin); + m_ndbvt.update(n.m_leaf, vol); + } + updateNormals(); + updateBounds(); + updateConstants(); + initializeDmInverse(); + } + + // update inertia tensor + updateLocalInertiaTensorFromNodes(); + + btMatrix3x3 id; + id.setIdentity(); + updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; + + internalInitialization(); +} + +void btReducedDeformableBody::setTotalMass(btScalar mass, bool fromfaces) +{ + // Changing the total mass after transform is applied is not allowed + btAssert(!m_transform_lock); + + btScalar scale_ratio = mass / m_mass; + + // update nodal mass + for (int i = 0; i < m_nFull; ++i) + { + m_nodalMass[i] *= scale_ratio; + } + m_mass = mass; + m_inverseMass = mass > 0 ? 1.0 / mass : 0; + + // update inertia tensors + updateLocalInertiaTensorFromNodes(); + + btMatrix3x3 id; + id.setIdentity(); + updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; + + internalInitialization(); +} + +void btReducedDeformableBody::updateRestNodalPositions() +{ + // update reset nodal position + m_x0.resize(m_nFull); + for (int i = 0; i < m_nFull; ++i) + { + m_x0[i] = m_nodes[i].m_x; + } +} + +// reference notes: +// https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec26.pdf +void btReducedDeformableBody::updateLocalInertiaTensorFromNodes() +{ + btMatrix3x3 inertia_tensor; + inertia_tensor.setZero(); + + for (int p = 0; p < m_nFull; ++p) + { + btMatrix3x3 particle_inertia; + particle_inertia.setZero(); + + btVector3 r = m_nodes[p].m_x - m_initialCoM; + + particle_inertia[0][0] = m_nodalMass[p] * (r[1] * r[1] + r[2] * r[2]); + particle_inertia[1][1] = m_nodalMass[p] * (r[0] * r[0] + r[2] * r[2]); + particle_inertia[2][2] = m_nodalMass[p] * (r[0] * r[0] + r[1] * r[1]); + + particle_inertia[0][1] = - m_nodalMass[p] * (r[0] * r[1]); + particle_inertia[0][2] = - m_nodalMass[p] * (r[0] * r[2]); + particle_inertia[1][2] = - m_nodalMass[p] * (r[1] * r[2]); + + particle_inertia[1][0] = particle_inertia[0][1]; + particle_inertia[2][0] = particle_inertia[0][2]; + particle_inertia[2][1] = particle_inertia[1][2]; + + inertia_tensor += particle_inertia; + } + m_invInertiaLocal = inertia_tensor.inverse(); +} + +void btReducedDeformableBody::updateInitialInertiaTensor(const btMatrix3x3& rotation) +{ + // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose(); + m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose(); +} + +void btReducedDeformableBody::updateModesByRotation(const btMatrix3x3& rotation) +{ + for (int r = 0; r < m_nReduced; ++r) + { + for (int i = 0; i < m_nFull; ++i) + { + btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + nodal_disp = rotation * nodal_disp; + + for (int k = 0; k < 3; ++k) + { + m_modes[r][3 * i + k] = nodal_disp[k]; + } + } + } +} + +void btReducedDeformableBody::updateInertiaTensor() +{ + m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose(); +} + +void btReducedDeformableBody::applyDamping(btScalar timeStep) +{ + m_linearVelocity *= btScalar(1) - m_linearDamping; + m_angularDamping *= btScalar(1) - m_angularDamping; +} + +void btReducedDeformableBody::applyCentralImpulse(const btVector3& impulse) +{ + m_linearVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif +} + +void btReducedDeformableBody::applyTorqueImpulse(const btVector3& torque) +{ + m_angularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif +} + +void btReducedDeformableBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos) +{ + if (m_inverseMass == btScalar(0.)) + { + std::cout << "something went wrong...probably didn't initialize?\n"; + btAssert(false); + } + // delta linear velocity + m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass; + // delta angular velocity + btVector3 torque = rel_pos.cross(impulse * m_linearFactor); + m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor; +} + +btVector3 btReducedDeformableBody::getRelativePos(int n_node) +{ + btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis(); + btVector3 ri = rotation * m_localMomentArm[n_node]; + return ri; +} + +btMatrix3x3 btReducedDeformableBody::getImpulseFactor(int n_node) +{ + // relative position + btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis(); + btVector3 ri = rotation * m_localMomentArm[n_node]; + btMatrix3x3 ri_skew = Cross(ri); + + // calculate impulse factor + // rigid part + btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0); + btMatrix3x3 K1 = Diagonal(inv_mass); + K1 -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew; + + // reduced deformable part + btMatrix3x3 SA; + SA.setZero(); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + for (int r = 0; r < m_nReduced; ++r) + { + SA[i][j] += m_modes[r][3 * n_node + i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]); + } + } + } + btMatrix3x3 RSARinv = rotation * SA * rotation.transpose(); + + + TVStack omega_helper; // Sum_i m_i r*_i R S_i + omega_helper.resize(m_nReduced); + for (int r = 0; r < m_nReduced; ++r) + { + omega_helper[r].setZero(); + for (int i = 0; i < m_nFull; ++i) + { + btMatrix3x3 mi_rstar_i = rotation * Cross(m_localMomentArm[i]) * m_nodalMass[i]; + btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + omega_helper[r] += mi_rstar_i * rotation * s_ri; + } + } + + btMatrix3x3 sum_multiply_A; + sum_multiply_A.setZero(); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + for (int r = 0; r < m_nReduced; ++r) + { + sum_multiply_A[i][j] += omega_helper[r][i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]); + } + } + } + + btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose(); + + return m_rigidOnly ? K1 : K1 + K2; +} + +void btReducedDeformableBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt) +{ + if (!m_rigidOnly) + { + // apply impulse force + applyFullSpaceNodalForce(impulse / dt, n_node); + + // update delta damping force + tDenseArray reduced_vel_tmp; + reduced_vel_tmp.resize(m_nReduced); + for (int r = 0; r < m_nReduced; ++r) + { + reduced_vel_tmp[r] = m_reducedVelocity[r] + m_internalDeltaReducedVelocity[r]; + } + applyReducedDampingForce(reduced_vel_tmp); + // applyReducedDampingForce(m_internalDeltaReducedVelocity); + + // delta reduced velocity + for (int r = 0; r < m_nReduced; ++r) + { + // The reduced mass is always identity! + m_internalDeltaReducedVelocity[r] += dt * (m_reducedForceDamping[r] + m_reducedForceExternal[r]); + } + } + + internalApplyRigidImpulse(impulse, rel_pos); +} + +void btReducedDeformableBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node) +{ + // f_local = R^-1 * f_ext //TODO: interpoalted transfrom + // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext; + btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext; + + // f_ext_r = [S^T * P]_{n_node} * f_local + tDenseArray f_ext_r; + f_ext_r.resize(m_nReduced, 0); + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceExternal[r] = 0; + for (int k = 0; k < 3; ++k) + { + f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k]; + } + + m_reducedForceExternal[r] += f_ext_r[r]; + } +} + +void btReducedDeformableBody::applyRigidGravity(const btVector3& gravity, btScalar dt) +{ + // update rigid frame velocity + m_linearVelocity += dt * gravity; +} + +void btReducedDeformableBody::applyReducedElasticForce(const tDenseArray& reduce_dofs) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r]; + } +} + +void btReducedDeformableBody::applyReducedDampingForce(const tDenseArray& reduce_vel) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r]; + } +} + +btScalar btReducedDeformableBody::getTotalMass() const +{ + return m_mass; +} + +btTransform& btReducedDeformableBody::getRigidTransform() +{ + return m_rigidTransformWorld; +} + +const btVector3& btReducedDeformableBody::getLinearVelocity() const +{ + return m_linearVelocity; +} + +const btVector3& btReducedDeformableBody::getAngularVelocity() const +{ + return m_angularVelocity; +} + +void btReducedDeformableBody::disableReducedModes(const bool rigid_only) +{ + m_rigidOnly = rigid_only; +} + +bool btReducedDeformableBody::isReducedModesOFF() const +{ + return m_rigidOnly; +} \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h new file mode 100644 index 00000000000..8968fb0cb97 --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h @@ -0,0 +1,257 @@ +#ifndef BT_REDUCED_SOFT_BODY_H +#define BT_REDUCED_SOFT_BODY_H + +#include "../btSoftBody.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btTransform.h" + +// Reduced deformable body is a simplified deformable object embedded in a rigid frame. +class btReducedDeformableBody : public btSoftBody +{ + public: + // + // Typedefs + // + typedef btAlignedObjectArray TVStack; + // typedef btAlignedObjectArray tBlockDiagMatrix; + typedef btAlignedObjectArray tDenseArray; + typedef btAlignedObjectArray > tDenseMatrix; + + private: + // flag to turn off the reduced modes + bool m_rigidOnly; + + // Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass. + bool m_transform_lock; + + // scaling factors + btScalar m_rhoScale; // mass density scale + btScalar m_ksScale; // stiffness scale + + // projection matrix + tDenseMatrix m_projPA; // Eqn. 4.11 from Rahul Sheth's thesis + tDenseMatrix m_projCq; + tDenseArray m_STP; + tDenseArray m_MrInvSTP; + + TVStack m_localMomentArm; // Sq + x0 + + btVector3 m_internalDeltaLinearVelocity; + btVector3 m_internalDeltaAngularVelocity; + tDenseArray m_internalDeltaReducedVelocity; + + btVector3 m_linearVelocityFromReduced; // contribution to the linear velocity from reduced velocity + btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity + btVector3 m_internalDeltaAngularVelocityFromReduced; + + protected: + // rigid frame + btScalar m_mass; // total mass of the rigid frame + btScalar m_inverseMass; // inverse of the total mass of the rigid frame + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btScalar m_linearDamping; // linear damping coefficient + btScalar m_angularDamping; // angular damping coefficient + btVector3 m_linearFactor; + btVector3 m_angularFactor; + // btVector3 m_invInertiaLocal; + btMatrix3x3 m_invInertiaLocal; + btTransform m_rigidTransformWorld; + btMatrix3x3 m_invInertiaTensorWorldInitial; + btMatrix3x3 m_invInertiaTensorWorld; + btMatrix3x3 m_interpolateInvInertiaTensorWorld; + btVector3 m_initialCoM; // initial center of mass (original of the m_rigidTransformWorld) + + // damping + btScalar m_dampingAlpha; + btScalar m_dampingBeta; + + public: + // + // Fields + // + + // reduced space + int m_nReduced; + int m_nFull; + tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes + tDenseArray m_reducedDofs; // Reduced degree of freedom + tDenseArray m_reducedDofsBuffer; // Reduced degree of freedom at t^n + tDenseArray m_reducedVelocity; // Reduced velocity array + tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n + tDenseArray m_reducedForceExternal; // reduced external force + tDenseArray m_reducedForceElastic; // reduced internal elastic force + tDenseArray m_reducedForceDamping; // reduced internal damping force + tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model + tDenseArray m_Kr; // reduced stiffness matrix + + // full space + TVStack m_x0; // Rest position + tDenseArray m_nodalMass; // Mass on each node + btAlignedObjectArray m_fixedNodes; // index of the fixed nodes + int m_nodeIndexOffset; // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world. + + // contacts + btAlignedObjectArray m_contactNodesList; + + // + // Api + // + btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); + + ~btReducedDeformableBody() {} + + // + // initializing helpers + // + void internalInitialization(); + + void setReducedModes(int num_modes, int full_size); + + void setMassProps(const tDenseArray& mass_array); + + void setInertiaProps(); + + void setRigidVelocity(const btVector3& v); + + void setRigidAngularVelocity(const btVector3& omega); + + void setStiffnessScale(const btScalar ks); + + void setMassScale(const btScalar rho); + + void setFixedNodes(const int n_node); + + void setDamping(const btScalar alpha, const btScalar beta); + + void disableReducedModes(const bool rigid_only); + + virtual void setTotalMass(btScalar mass, bool fromfaces = false); + + // + // various internal updates + // + virtual void transformTo(const btTransform& trs); + virtual void transform(const btTransform& trs); + // caution: + // need to use scale before using transform, because the scale is performed in the local frame + // (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info) + virtual void scale(const btVector3& scl); + + private: + void updateRestNodalPositions(); + + void updateInitialInertiaTensor(const btMatrix3x3& rotation); + + void updateLocalInertiaTensorFromNodes(); + + void updateInertiaTensor(); + + void updateModesByRotation(const btMatrix3x3& rotation); + + public: + void updateLocalMomentArm(); + + void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform); + + // update the external force projection matrix + void updateExternalForceProjectMatrix(bool initialized); + + void endOfTimeStepZeroing(); + + void applyInternalVelocityChanges(); + + // + // position and velocity update related + // + + // compute reduced degree of freedoms + void updateReducedDofs(btScalar solverdt); + + // compute reduced velocity update (for explicit time stepping) + void updateReducedVelocity(btScalar solverdt); + + // map to full degree of freedoms + void mapToFullPosition(const btTransform& ref_trans); + + // compute full space velocity from the reduced velocity + void mapToFullVelocity(const btTransform& ref_trans); + + // compute total angular momentum + const btVector3 computeTotalAngularMomentum() const; + + // get a single node's full space velocity from the reduced velocity + const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const; + + // get a single node's all delta velocity + const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const; + + // + // rigid motion related + // + void applyDamping(btScalar timeStep); + + void applyCentralImpulse(const btVector3& impulse); + + void applyTorqueImpulse(const btVector3& torque); + + void proceedToTransform(btScalar dt, bool end_of_time_step); + + // + // force related + // + + // apply impulse to the rigid frame + void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos); + + // apply impulse to nodes in the full space + void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt); + + // apply nodal external force in the full space + void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node); + + // apply gravity to the rigid frame + void applyRigidGravity(const btVector3& gravity, btScalar dt); + + // apply reduced elastic force + void applyReducedElasticForce(const tDenseArray& reduce_dofs); + + // apply reduced damping force + void applyReducedDampingForce(const tDenseArray& reduce_vel); + + // calculate the impulse factor + virtual btMatrix3x3 getImpulseFactor(int n_node); + + // get relative position from a node to the CoM of the rigid frame + btVector3 getRelativePos(int n_node); + + // + // accessors + // + bool isReducedModesOFF() const; + btScalar getTotalMass() const; + btTransform& getRigidTransform(); + const btVector3& getLinearVelocity() const; + const btVector3& getAngularVelocity() const; + + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + void clampVelocity(btVector3& v) const { + v.setX( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getX())) + ); + v.setY( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getY())) + ); + v.setZ( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) + ); + } + #endif +}; + +#endif // BT_REDUCED_SOFT_BODY_H \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp new file mode 100644 index 00000000000..0f95bc53bfb --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp @@ -0,0 +1,215 @@ +#include "btReducedDeformableBodyHelpers.h" +#include "../btSoftBodyHelpers.h" +#include +#include +#include + +btReducedDeformableBody* btReducedDeformableBodyHelpers::createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only) { + std::string filename = file_path + vtk_file; + btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createFromVtkFile(worldInfo, filename.c_str()); + + rsb->setReducedModes(num_modes, rsb->m_nodes.size()); + btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str()); + + rsb->disableReducedModes(rigid_only); + + return rsb; +} + +btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) +{ + std::ifstream fs; + fs.open(vtk_file); + btAssert(fs); + + typedef btAlignedObjectArray Index; + std::string line; + btAlignedObjectArray X; + btVector3 position; + btAlignedObjectArray indices; + bool reading_points = false; + bool reading_tets = false; + size_t n_points = 0; + size_t n_tets = 0; + size_t x_count = 0; + size_t indices_count = 0; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line.size() == (size_t)(0)) + { + } + else if (line.substr(0, 6) == "POINTS") + { + reading_points = true; + reading_tets = false; + ss.ignore(128, ' '); // ignore "POINTS" + ss >> n_points; + X.resize(n_points); + } + else if (line.substr(0, 5) == "CELLS") + { + reading_points = false; + reading_tets = true; + ss.ignore(128, ' '); // ignore "CELLS" + ss >> n_tets; + indices.resize(n_tets); + } + else if (line.substr(0, 10) == "CELL_TYPES") + { + reading_points = false; + reading_tets = false; + } + else if (reading_points) + { + btScalar p; + ss >> p; + position.setX(p); + ss >> p; + position.setY(p); + ss >> p; + position.setZ(p); + //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ()); + X[x_count++] = position; + } + else if (reading_tets) + { + int d; + ss >> d; + if (d != 4) + { + printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n"); + fs.close(); + return 0; + } + ss.ignore(128, ' '); // ignore "4" + Index tet; + tet.resize(4); + for (size_t i = 0; i < 4; i++) + { + ss >> tet[i]; + //printf("%d ", tet[i]); + } + //printf("\n"); + indices[indices_count++] = tet; + } + } + btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0); + + for (int i = 0; i < n_tets; ++i) + { + const Index& ni = indices[i]; + rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + { + rsb->appendLink(ni[0], ni[1], 0, true); + rsb->appendLink(ni[1], ni[2], 0, true); + rsb->appendLink(ni[2], ni[0], 0, true); + rsb->appendLink(ni[0], ni[3], 0, true); + rsb->appendLink(ni[1], ni[3], 0, true); + rsb->appendLink(ni[2], ni[3], 0, true); + } + } + + btSoftBodyHelpers::generateBoundaryFaces(rsb); + rsb->initializeDmInverse(); + rsb->m_tetraScratches.resize(rsb->m_tetras.size()); + rsb->m_tetraScratchesTn.resize(rsb->m_tetras.size()); + printf("Nodes: %u\r\n", rsb->m_nodes.size()); + printf("Links: %u\r\n", rsb->m_links.size()); + printf("Faces: %u\r\n", rsb->m_faces.size()); + printf("Tetras: %u\r\n", rsb->m_tetras.size()); + + fs.close(); + + return rsb; +} + +void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path) +{ + // read in eigenmodes, stiffness and mass matrices + std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin"; + btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_eigenvalues, rsb->m_nReduced, eigenvalues_file.c_str()); + + std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin"; + btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Kr, rsb->m_nReduced, Kr_file.c_str()); + + // std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin"; + // btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Mr, rsb->m_nReduced, Mr_file.c_str()); + + std::string modes_file = std::string(file_path) + "modes.bin"; + btReducedDeformableBodyHelpers::readBinaryMat(rsb->m_modes, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str()); + + // read in full nodal mass + std::string M_file = std::string(file_path) + "M_diag_mat.bin"; + btAlignedObjectArray mass_array; + btReducedDeformableBodyHelpers::readBinaryVec(mass_array, rsb->m_nFull, M_file.c_str()); + rsb->setMassProps(mass_array); + + // calculate the inertia tensor in the local frame + rsb->setInertiaProps(); + + // other internal initialization + rsb->internalInitialization(); +} + +// read in a vector from the binary file +void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec, + const unsigned int n_size, // #entries read + const char* file) +{ + std::ifstream f_in(file, std::ios::in | std::ios::binary); + // first get size + unsigned int size=0; + f_in.read((char*)&size, 4);//sizeof(unsigned int)); + btAssert(size >= n_size); // make sure the #requested mode is smaller than the #available modes + + // read data + vec.resize(n_size); + double temp; + for (unsigned int i = 0; i < n_size; ++i) + { + f_in.read((char*)&temp, sizeof(double)); + vec[i] = btScalar(temp); + } + f_in.close(); +} + +// read in a matrix from the binary file +void btReducedDeformableBodyHelpers::readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, + const unsigned int n_modes, // #modes, outer array size + const unsigned int n_full, // inner array size + const char* file) +{ + std::ifstream f_in(file, std::ios::in | std::ios::binary); + // first get size + unsigned int v_size=0; + f_in.read((char*)&v_size, 4);//sizeof(unsigned int)); + btAssert(v_size >= n_modes * n_full); // make sure the #requested mode is smaller than the #available modes + + // read data + mat.resize(n_modes); + for (int i = 0; i < n_modes; ++i) + { + for (int j = 0; j < n_full; ++j) + { + double temp; + f_in.read((char*)&temp, sizeof(double)); + + if (mat[i].size() != n_modes) + mat[i].resize(n_full); + mat[i][j] = btScalar(temp); + } + } + f_in.close(); +} + +void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin) +{ + btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]); + btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]); + btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]); + + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); +} diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h new file mode 100644 index 00000000000..2b259a931fe --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h @@ -0,0 +1,25 @@ +#ifndef BT_REDUCED_SOFT_BODY_HELPERS_H +#define BT_REDUCED_SOFT_BODY_HELPERS_H + +#include "btReducedDeformableBody.h" +#include + +struct btReducedDeformableBodyHelpers +{ + // create a reduced deformable object + static btReducedDeformableBody* createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only); + // read in geometry info from Vtk file + static btReducedDeformableBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + // read in all reduced files + static void readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path); + // read in a binary vector + static void readBinaryVec(btReducedDeformableBody::tDenseArray& vec, const unsigned int n_size, const char* file); + // read in a binary matrix + static void readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, const unsigned int n_modes, const unsigned int n_full, const char* file); + + // calculate the local inertia tensor for a box shape reduced deformable object + static void calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin); +}; + + +#endif // BT_REDUCED_SOFT_BODY_HELPERS_H \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp new file mode 100644 index 00000000000..1418cc24764 --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp @@ -0,0 +1,325 @@ +#include "btReducedDeformableBodySolver.h" +#include "btReducedDeformableBody.h" + +btReducedDeformableBodySolver::btReducedDeformableBodySolver() +{ + m_ascendOrder = true; + m_reducedSolver = true; + m_dampingAlpha = 0; + m_dampingBeta = 0; + m_gravity = btVector3(0, 0, 0); +} + +void btReducedDeformableBodySolver::setGravity(const btVector3& gravity) +{ + m_gravity = gravity; +} + +void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray& bodies, btScalar dt) +{ + m_softBodies.copyFromArray(bodies); + bool nodeUpdated = updateNodes(); + + if (nodeUpdated) + { + m_dv.resize(m_numNodes, btVector3(0, 0, 0)); + m_ddv.resize(m_numNodes, btVector3(0, 0, 0)); + m_residual.resize(m_numNodes, btVector3(0, 0, 0)); + m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0)); + } + + // need to setZero here as resize only set value for newly allocated items + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i].setZero(); + m_ddv[i].setZero(); + m_residual[i].setZero(); + } + + if (dt > 0) + { + m_dt = dt; + } + m_objective->reinitialize(nodeUpdated, dt); + + int N = bodies.size(); + if (nodeUpdated) + { + m_staticConstraints.resize(N); + m_nodeRigidConstraints.resize(N); + // m_faceRigidConstraints.resize(N); + } + for (int i = 0; i < N; ++i) + { + m_staticConstraints[i].clear(); + m_nodeRigidConstraints[i].clear(); + // m_faceRigidConstraints[i].clear(); + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + rsb->m_contactNodesList.clear(); + } + + // set node index offsets + int sum = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + rsb->m_nodeIndexOffset = sum; + sum += rsb->m_nodes.size(); + } + + btDeformableBodySolver::updateSoftBodies(); +} + +void btReducedDeformableBodySolver::predictMotion(btScalar solverdt) +{ + applyExplicitForce(solverdt); + + // predict new mesh location + predictReduceDeformableMotion(solverdt); + + //TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion +} + +void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + if (!rsb->isActive()) + { + continue; + } + + // clear contacts variables + rsb->m_nodeRigidContacts.resize(0); + rsb->m_faceRigidContacts.resize(0); + rsb->m_faceNodeContacts.resize(0); + + // calculate inverse mass matrix for all nodes + for (int j = 0; j < rsb->m_nodes.size(); ++j) + { + if (rsb->m_nodes[j].m_im > 0) + { + rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse(); + } + } + + // rigid motion: t, R at time^* + rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); + + // update reduced dofs at time^* + // rsb->updateReducedDofs(solverdt); + + // update local moment arm at time^* + // rsb->updateLocalMomentArm(); + // rsb->updateExternalForceProjectMatrix(true); + + // predict full space velocity at time^* (needed for constraints) + rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform()); + + // update full space nodal position at time^* + rsb->mapToFullPosition(rsb->getInterpolationWorldTransform()); + + // update bounding box + rsb->updateBounds(); + + // update tree + rsb->updateNodeTree(true, true); + if (!rsb->m_fdbvt.empty()) + { + rsb->updateFaceTree(true, true); + } + } +} + +void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + + // apply gravity to the rigid frame, get m_linearVelocity at time^* + rsb->applyRigidGravity(m_gravity, solverdt); + + if (!rsb->isReducedModesOFF()) + { + // add internal force (elastic force & damping force) + rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); + rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); + + // get reduced velocity at time^* + rsb->updateReducedVelocity(solverdt); + } + + // apply damping (no need at this point) + // rsb->applyDamping(solverdt); + } +} + +void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + + // rigid motion + rsb->proceedToTransform(timeStep, true); + + if (!rsb->isReducedModesOFF()) + { + // update reduced dofs for time^n+1 + rsb->updateReducedDofs(timeStep); + + // update local moment arm for time^n+1 + rsb->updateLocalMomentArm(); + rsb->updateExternalForceProjectMatrix(true); + } + + // update mesh nodal positions for time^n+1 + rsb->mapToFullPosition(rsb->getRigidTransform()); + + // update mesh nodal velocity + rsb->mapToFullVelocity(rsb->getRigidTransform()); + + // end of time step clean up and update + rsb->endOfTimeStepZeroing(); + + // update the rendering mesh + rsb->interpolateRenderMesh(); + } +} + +void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + if (!rsb->isActive()) + { + continue; + } + + // set fixed constraints + for (int j = 0; j < rsb->m_fixedNodes.size(); ++j) + { + int i_node = rsb->m_fixedNodes[j]; + if (rsb->m_nodes[i_node].m_im == 0) + { + for (int k = 0; k < 3; ++k) + { + btVector3 dir(0, 0, 0); + dir[k] = 1; + btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt); + m_staticConstraints[i].push_back(static_constraint); + } + } + } + btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size()); + + // set Deformable Node vs. Rigid constraint + for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j) + { + const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j]; + // skip fixed points + if (contact.m_node->m_im == 0) + { + continue; + } + btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt); + m_nodeRigidConstraints[i].push_back(constraint); + rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset); + } + // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n"; + // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; + + } +} + +btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + btScalar residualSquare = 0; + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btAlignedObjectArray m_orderNonContactConstraintPool; + btAlignedObjectArray m_orderContactConstraintPool; + + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + + // shuffle the order of applying constraint + m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size()); + m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size()); + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) + { + // fixed constraint order + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j; + } + // contact constraint order + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j; + } + + m_ascendOrder = m_ascendOrder ? false : true; + } + else + { + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + m_orderNonContactConstraintPool[j] = j; + } + // contact constraint order + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + m_orderContactConstraintPool[j] = j; + } + } + + // handle fixed constraint + for (int k = 0; k < m_staticConstraints[i].size(); ++k) + { + btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + + // handle contact constraint + + // node vs rigid contact + // std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n'; + for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k) + { + btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + + // face vs rigid contact + // for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k) + // { + // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k]; + // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + // residualSquare = btMax(residualSquare, localResidualSquare); + // } + } + + + return residualSquare; +} + +void btReducedDeformableBodySolver::deformableBodyInternalWriteBack() +{ + // reduced deformable update + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedDeformableBody* rsb = static_cast(m_softBodies[i]); + rsb->applyInternalVelocityChanges(); + } + m_ascendOrder = true; +} \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h new file mode 100644 index 00000000000..04c171f3159 --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h @@ -0,0 +1,61 @@ +#ifndef BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H +#define BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H + +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "btReducedDeformableContactConstraint.h" + +class btReducedDeformableBody; + +class btReducedDeformableBodySolver : public btDeformableBodySolver +{ + protected: + bool m_ascendOrder; + btScalar m_dampingAlpha; + btScalar m_dampingBeta; + + btVector3 m_gravity; + + void predictReduceDeformableMotion(btScalar solverdt); + + void applyExplicitForce(btScalar solverdt); + + public: + btAlignedObjectArray > m_staticConstraints; + btAlignedObjectArray > m_nodeRigidConstraints; + btAlignedObjectArray > m_faceRigidConstraints; + + btReducedDeformableBodySolver(); + ~btReducedDeformableBodySolver() {} + + virtual void setGravity(const btVector3& gravity); + + virtual SolverTypes getSolverType() const + { + return REDUCED_DEFORMABLE_SOLVER; + } + + // resize/clear data structures + virtual void reinitialize(const btAlignedObjectArray& bodies, btScalar dt); + + virtual void predictMotion(btScalar solverdt); + + virtual void applyTransforms(btScalar timeStep); + + // set up contact constraints + virtual void setConstraints(const btContactSolverInfo& infoGlobal); + + // solve all constraints (fixed and contact) + virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // apply all the delta velocities + virtual void deformableBodyInternalWriteBack(); + + // virtual void setProjection() {} + + // virtual void setLagrangeMultiplier() {} + + // virtual void setupDeformableSolve(bool implicit); + +}; + +#endif // BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp new file mode 100644 index 00000000000..3c78d2d225f --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp @@ -0,0 +1,579 @@ +#include "btReducedDeformableContactConstraint.h" +#include + +// ================= static constraints =================== +btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint( + btReducedDeformableBody* rsb, + btSoftBody::Node* node, + const btVector3& ri, + const btVector3& x0, + const btVector3& dir, + const btContactSolverInfo& infoGlobal, + btScalar dt) + : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal) +{ + m_erp = 0.2; + m_appliedImpulse = 0; + + // get impulse factor + m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index); + m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection); + + btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection); + btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection); + + m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor; +} + +btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) +{ + // target velocity of fixed constraint is 0 + btVector3 deltaVa = getDeltaVa(); + btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection); + btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor; + m_appliedImpulse = m_appliedImpulse + deltaImpulse; + + btVector3 impulse = deltaImpulse * m_impulseDirection; + applyImpulse(impulse); + + // calculate residual + btScalar residualSquare = m_impulseFactor * deltaImpulse; + residualSquare *= residualSquare; + + return residualSquare; +} + +// this calls reduced deformable body's internalApplyFullSpaceImpulse +void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse) +{ + // apply full space impulse + m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); +} + +btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const +{ + return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index); +} + +// ================= base contact constraints =================== +btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint( + btReducedDeformableBody* rsb, + const btSoftBody::DeformableRigidContact& c, + const btContactSolverInfo& infoGlobal, + btScalar dt) + : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal) +{ + m_nodeQueryIndex = 0; + m_appliedNormalImpulse = 0; + m_appliedTangentImpulse = 0; + m_rhs = 0; + m_rhs_tangent = 0; + m_cfm = infoGlobal.m_deformable_cfm; + m_cfm_friction = 0; + m_erp = infoGlobal.m_deformable_erp; + m_erp_friction = infoGlobal.m_deformable_erp; + m_friction = infoGlobal.m_friction; + + m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject(); + m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK); +} + +void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body) +{ + if (!m_collideMultibody) + { + m_solverBodyId = bodyId; + m_solverBody = &solver_body; + m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass(); + btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA); + m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis; + + m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass(); + btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent); + m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent; + } +} + +btVector3 btReducedDeformableRigidContactConstraint::getVa() const +{ + btVector3 Va(0, 0, 0); + if (!m_collideStatic) + { + Va = btDeformableRigidContactConstraint::getVa(); + } + return Va; +} + +btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) +{ + // btVector3 Va = getVa(); + // btVector3 deltaVa = Va - m_bufferVelocityA; + // if (!m_collideStatic) + // { + // std::cout << "moving collision!!!\n"; + // std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n"; + // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t' + // << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t' + // << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n'; + // } + btVector3 deltaVa = getDeltaVa(); + btVector3 deltaVb = getDeltaVb(); + + // if (!m_collideStatic) + // { + // std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n'; + // std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n'; + // } + + // get delta relative velocity and magnitude (i.e., how much impulse has been applied?) + btVector3 deltaV_rel = deltaVa - deltaVb; + btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA); + + // if (!m_collideStatic) + // { + // std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n"; + // std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n"; + // std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n'; + // } + + // get the normal impulse to be applied + btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor; + // if (!m_collideStatic) + // { + // std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n"; + // std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n'; + // } + + { + // cumulative impulse that has been applied + btScalar sum = m_appliedNormalImpulse + deltaImpulse; + // if the cumulative impulse is pushing the object into the rigid body, set it zero + if (sum < 0) + { + deltaImpulse = -m_appliedNormalImpulse; + m_appliedNormalImpulse = 0; + } + else + { + m_appliedNormalImpulse = sum; + } + } + + // if (!m_collideStatic) + // { + // std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n'; + // std::cout << "deltaImpulse: " << deltaImpulse << '\n'; + // } + + // residual is the nodal normal velocity change in current iteration + btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual + residualSquare *= residualSquare; + + + // apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|) + btScalar deltaImpulse_tangent = 0; + btScalar deltaImpulse_tangent2 = 0; + { + // calculate how much impulse is needed + // btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent); + // btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv; + // deltaImpulse_tangent = m_rhs_tangent - impulse_changed; + + // btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent; + btScalar lower_limit = - m_appliedNormalImpulse * m_friction; + btScalar upper_limit = m_appliedNormalImpulse * m_friction; + // if (sum > upper_limit) + // { + // deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse; + // m_appliedTangentImpulse = upper_limit; + // } + // else if (sum < lower_limit) + // { + // deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse; + // m_appliedTangentImpulse = lower_limit; + // } + // else + // { + // m_appliedTangentImpulse = sum; + // } + // + calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent, + m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel); + + if (m_collideMultibody) + { + calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2, + m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel); + } + + + if (!m_collideStatic) + { + // std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n"; + // std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n'; + // std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n'; + // std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n'; + } + } + + // get the total impulse vector + btVector3 impulse_normal = deltaImpulse * m_contactNormalA; + btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent); + btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2); + btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2; + + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (!m_collideStatic) + { + // std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t' + // << m_linearComponentNormal[1] << '\t' + // << m_linearComponentNormal[2] << '\n'; + // std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t' + // << m_angularComponentNormal[1] << '\t' + // << m_angularComponentNormal[2] << '\n'; + + if (!m_collideMultibody) // collision with rigid body + { + // std::cout << "rigid impulse applied!!\n"; + // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; + // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; + + m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse); + m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent); + + // std::cout << "after\n"; + // std::cout << "rigid impulse applied!!\n"; + // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' + // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; + // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' + // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; + } + else // collision with multibody + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse); + + // const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + // std::cout << "deltaV_normal: "; + // for (int i = 0; i < ndof; ++i) + // { + // std::cout << i << "\t" << deltaV_normal[i] << '\n'; + // } + + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2); + } + } + } + } + return residualSquare; +} + +void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent, + btScalar& appliedImpulse, + const btScalar rhs_tangent, + const btScalar tangentImpulseFactorInv, + const btVector3& tangent, + const btScalar lower_limit, + const btScalar upper_limit, + const btVector3& deltaV_rel) +{ + btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent); + btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv; + deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed; + + btScalar sum = appliedImpulse + deltaImpulse_tangent; + if (sum > upper_limit) + { + deltaImpulse_tangent = upper_limit - appliedImpulse; + appliedImpulse = upper_limit; + } + else if (sum < lower_limit) + { + deltaImpulse_tangent = lower_limit - appliedImpulse; + appliedImpulse = lower_limit; + } + else + { + appliedImpulse = sum; + } +} + +// ================= node vs rigid constraints =================== +btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint( + btReducedDeformableBody* rsb, + const btSoftBody::DeformableNodeRigidContact& contact, + const btContactSolverInfo& infoGlobal, + btScalar dt) + : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt) +{ + m_contactNormalA = contact.m_cti.m_normal; + m_contactNormalB = -contact.m_cti.m_normal; + + if (contact.m_node->index < rsb->m_nodes.size()) + { + m_nodeQueryIndex = contact.m_node->index; + } + else + { + m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset; + } + + if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + m_relPosA = contact.m_c1; + } + else + { + m_relPosA = btVector3(0,0,0); + } + m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin(); + + if (m_collideStatic) // colliding with static object, only consider reduced deformable body's impulse factor + { + m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex); + } + else // colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors + { + m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0; + } + + m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA); + m_tangentImpulseFactor = 0; + + warmStarting(); +} + +void btReducedDeformableNodeRigidContactConstraint::warmStarting() +{ + btVector3 va = getVa(); + btVector3 vb = getVb(); + m_bufferVelocityA = va; + m_bufferVelocityB = vb; + + // we define the (+) direction of errors to be the outward surface normal of the rigid object + btVector3 v_rel = vb - va; + // get tangent direction of the relative velocity + btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA; + if (v_tangent.norm() < SIMD_EPSILON) + { + m_contactTangent = btVector3(0, 0, 0); + // tangent impulse factor + m_tangentImpulseFactor = 0; + m_tangentImpulseFactorInv = 0; + } + else + { + if (!m_collideMultibody) + { + m_contactTangent = v_tangent.normalized(); + m_contactTangent2.setZero(); + // tangent impulse factor 1 + m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent); + m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor; + // tangent impulse factor 2 + m_tangentImpulseFactor2 = 0; + m_tangentImpulseFactorInv2 = 0; + } + else // multibody requires 2 contact directions + { + m_contactTangent = m_contact->t1; + m_contactTangent2 = m_contact->t2; + + // tangent impulse factor 1 + m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent); + m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor; + // tangent impulse factor 2 + m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2); + m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2; + } + } + + + // initial guess for normal impulse + { + btScalar velocity_error = btDot(v_rel, m_contactNormalA); // magnitude of relative velocity + btScalar position_error = 0; + if (m_penetration > 0) + { + velocity_error += m_penetration / m_dt; + } + else + { + // add penetration correction vel + position_error = m_penetration * m_erp / m_dt; + } + // get the initial estimate of impulse magnitude to be applied + m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor; + } + + // initial guess for tangential impulse + { + btScalar velocity_error = btDot(v_rel, m_contactTangent); + m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv; + + if (m_collideMultibody) + { + btScalar velocity_error2 = btDot(v_rel, m_contactTangent2); + m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2; + } + } + + // warm starting + // applyImpulse(m_rhs * m_contactNormalA); + // if (!m_collideStatic) + // { + // const btSoftBody::sCti& cti = m_contact->m_cti; + // if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + // { + // m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs); + // } + // } +} + +btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const +{ + return m_node->m_v; +} + +btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const +{ + btVector3 deltaVa(0, 0, 0); + if (!m_collideStatic) + { + if (!m_collideMultibody) // for rigid body + { + deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA); + } + else // for multibody + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0; + for (int k = 0; k < ndof; ++k) + { + vel += local_dv[k] * J_n[k]; + } + deltaVa = m_contact->m_cti.m_normal * vel; + + // add in the tangential components of the va + vel = 0; + for (int k = 0; k < ndof; ++k) + { + vel += local_dv[k] * J_t1[k]; + } + deltaVa += m_contact->t1 * vel; + + vel = 0; + for (int k = 0; k < ndof; ++k) + { + vel += local_dv[k] * J_t2[k]; + } + deltaVa += m_contact->t2 * vel; + } + } + } + return deltaVa; +} + +btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const +{ + // std::cout << "node: " << m_node->index << '\n'; + return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex); +} + +btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const +{ + return m_node->m_splitv; +} + +btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const +{ + return m_total_normal_dv + m_total_tangent_dv; +} + +void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt); + // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt); + // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); + // if (!m_collideStatic) + // { + // // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; + // // std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; + // btVector3 v_after = getDeltaVb() + m_node->m_v; + // // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; + // } + // std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n'; +} + +// ================= face vs rigid constraints =================== +btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint( + btReducedDeformableBody* rsb, + const btSoftBody::DeformableFaceRigidContact& contact, + const btContactSolverInfo& infoGlobal, + btScalar dt, + bool useStrainLimiting) + : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt) +{} + +btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; +} + +btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2]; + return vb; +} + +btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const +{ + btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + if (m_face->m_n[0] == node) + { + return face_dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == node) + { + return face_dv * contact->m_weights[1]; + } + btAssert(node == m_face->m_n[2]); + return face_dv * contact->m_weights[2]; +} + +void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + // +} \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h new file mode 100644 index 00000000000..10d0938c5d2 --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h @@ -0,0 +1,194 @@ +#include "../btDeformableContactConstraint.h" +#include "btReducedDeformableBody.h" + +// ================= static constraints =================== +class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint +{ + public: + btReducedDeformableBody* m_rsb; + btScalar m_dt; + btVector3 m_ri; + btVector3 m_targetPos; + btVector3 m_impulseDirection; + btMatrix3x3 m_impulseFactorMatrix; + btScalar m_impulseFactor; + btScalar m_rhs; + btScalar m_appliedImpulse; + btScalar m_erp; + + btReducedDeformableStaticConstraint(btReducedDeformableBody* rsb, + btSoftBody::Node* node, + const btVector3& ri, + const btVector3& x0, + const btVector3& dir, + const btContactSolverInfo& infoGlobal, + btScalar dt); + // btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other); + btReducedDeformableStaticConstraint() {} + virtual ~btReducedDeformableStaticConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // this calls reduced deformable body's applyFullSpaceImpulse + virtual void applyImpulse(const btVector3& impulse); + + btVector3 getDeltaVa() const; + + // virtual void applySplitImpulse(const btVector3& impulse) {} +}; + +// ================= base contact constraints =================== +class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint +{ + public: + bool m_collideStatic; // flag for collision with static object + bool m_collideMultibody; // flag for collision with multibody + + int m_nodeQueryIndex; + int m_solverBodyId; // for debugging + + btReducedDeformableBody* m_rsb; + btSolverBody* m_solverBody; + btScalar m_dt; + + btScalar m_appliedNormalImpulse; + btScalar m_appliedTangentImpulse; + btScalar m_appliedTangentImpulse2; + btScalar m_normalImpulseFactor; + btScalar m_tangentImpulseFactor; + btScalar m_tangentImpulseFactor2; + btScalar m_tangentImpulseFactorInv; + btScalar m_tangentImpulseFactorInv2; + btScalar m_rhs; + btScalar m_rhs_tangent; + btScalar m_rhs_tangent2; + + btScalar m_cfm; + btScalar m_cfm_friction; + btScalar m_erp; + btScalar m_erp_friction; + btScalar m_friction; + + btVector3 m_contactNormalA; // surface normal for rigid body (opposite direction as impulse) + btVector3 m_contactNormalB; // surface normal for reduced deformable body (opposite direction as impulse) + btVector3 m_contactTangent; // tangential direction of the relative velocity + btVector3 m_contactTangent2; // 2nd tangential direction of the relative velocity + btVector3 m_relPosA; // relative position of the contact point for A (rigid) + btVector3 m_relPosB; // relative position of the contact point for B + btMatrix3x3 m_impulseFactor; // total impulse matrix + + btVector3 m_bufferVelocityA; // velocity at the beginning of the iteration + btVector3 m_bufferVelocityB; + btVector3 m_linearComponentNormal; // linear components for the solver body + btVector3 m_angularComponentNormal; // angular components for the solver body + // since 2nd contact direction only applies to multibody, these components will never be used + btVector3 m_linearComponentTangent; + btVector3 m_angularComponentTangent; + + btReducedDeformableRigidContactConstraint(btReducedDeformableBody* rsb, + const btSoftBody::DeformableRigidContact& c, + const btContactSolverInfo& infoGlobal, + btScalar dt); + // btReducedDeformableRigidContactConstraint(const btReducedDeformableRigidContactConstraint& other); + btReducedDeformableRigidContactConstraint() {} + virtual ~btReducedDeformableRigidContactConstraint() {} + + void setSolverBody(const int bodyId, btSolverBody& solver_body); + + virtual void warmStarting() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + void calculateTangentialImpulse(btScalar& deltaImpulse_tangent, + btScalar& appliedImpulse, + const btScalar rhs_tangent, + const btScalar tangentImpulseFactorInv, + const btVector3& tangent, + const btScalar lower_limit, + const btScalar upper_limit, + const btVector3& deltaV_rel); + + virtual void applyImpulse(const btVector3& impulse) {} + + virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later + + virtual btVector3 getVa() const; + virtual btVector3 getDeltaVa() const = 0; + virtual btVector3 getDeltaVb() const = 0; +}; + +// ================= node vs rigid constraints =================== +class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformableRigidContactConstraint +{ + public: + btSoftBody::Node* m_node; + + btReducedDeformableNodeRigidContactConstraint(btReducedDeformableBody* rsb, + const btSoftBody::DeformableNodeRigidContact& contact, + const btContactSolverInfo& infoGlobal, + btScalar dt); + // btReducedDeformableNodeRigidContactConstraint(const btReducedDeformableNodeRigidContactConstraint& other); + btReducedDeformableNodeRigidContactConstraint() {} + virtual ~btReducedDeformableNodeRigidContactConstraint() {} + + virtual void warmStarting(); + + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + + // get the velocity change of the rigid body + virtual btVector3 getDeltaVa() const; + + // get velocity change of the node in contat + virtual btVector3 getDeltaVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast(m_contact); + } + + // this calls reduced deformable body's applyFullSpaceImpulse + virtual void applyImpulse(const btVector3& impulse); +}; + +// ================= face vs rigid constraints =================== +class btReducedDeformableFaceRigidContactConstraint : public btReducedDeformableRigidContactConstraint +{ + public: + btSoftBody::Face* m_face; + bool m_useStrainLimiting; + + btReducedDeformableFaceRigidContactConstraint(btReducedDeformableBody* rsb, + const btSoftBody::DeformableFaceRigidContact& contact, + const btContactSolverInfo& infoGlobal, + btScalar dt, + bool useStrainLimiting); + // btReducedDeformableFaceRigidContactConstraint(const btReducedDeformableFaceRigidContactConstraint& other); + btReducedDeformableFaceRigidContactConstraint() {} + virtual ~btReducedDeformableFaceRigidContactConstraint() {} + + // get the velocity of the deformable face at the contact point + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceRigidContact* getContact() const + { + return static_cast(m_contact); + } + + // this calls reduced deformable body's applyFullSpaceImpulse + virtual void applyImpulse(const btVector3& impulse); +}; \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h index eb05b9f010c..60b6fe38821 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -25,12 +25,18 @@ #include "btDeformableNeoHookeanForce.h" #include "btDeformableContactProjection.h" #include "btPreconditioner.h" -#include "btDeformableMultiBodyDynamicsWorld.h" +// #include "btDeformableMultiBodyDynamicsWorld.h" #include "LinearMath/btQuickprof.h" class btDeformableBackwardEulerObjective { public: + enum _ + { + Mass_preconditioner, + KKT_preconditioner + }; + typedef btAlignedObjectArray TVStack; btScalar m_dt; btAlignedObjectArray m_lf; diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp index 699c6bcad44..4e9df5f83e2 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp @@ -23,6 +23,7 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); + m_reducedSolver = false; } btDeformableBodySolver::~btDeformableBodySolver() @@ -401,7 +402,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) } } } - m_objective->applyExplicitForce(m_residual); + applyExplicitForce(); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -411,6 +412,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) psb->m_nodeRigidContacts.resize(0); psb->m_faceRigidContacts.resize(0); psb->m_faceNodeContacts.resize(0); + psb->m_faceNodeContactsCCD.resize(0); // predict motion for collision detection predictDeformableMotion(psb, solverdt); } @@ -503,3 +505,89 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch) { m_lineSearch = lineSearch; } + +void btDeformableBodySolver::applyExplicitForce() +{ + m_objective->applyExplicitForce(m_residual); +} + +void btDeformableBodySolver::applyTransforms(btScalar timeStep) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } +} \ No newline at end of file diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h index ae674d6e892..68354f1996b 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h @@ -24,8 +24,8 @@ #include "btConjugateResidual.h" #include "btConjugateGradient.h" struct btCollisionObjectWrapper; -class btDeformableBackwardEulerObjective; -class btDeformableMultiBodyDynamicsWorld; +// class btDeformableBackwardEulerObjective; +// class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { @@ -46,6 +46,7 @@ protected: int m_maxNewtonIterations; // max number of newton iterations btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance bool m_lineSearch; // If true, use newton's method with line search under implicit scheme + bool m_reducedSolver; // flag for reduced soft body solver public: // handles data related to objective function btDeformableBackwardEulerObjective* m_objective; @@ -68,11 +69,18 @@ public: // solve the momentum equation virtual void solveDeformableConstraints(btScalar solverdt); + // set gravity (get from deformable world) + virtual void setGravity(const btVector3& gravity) + { + // for full deformable object, we don't store gravity in the solver + // this function is overriden in the reduced deformable object + } + // resize/clear data structures - void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); + virtual void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); // set up contact constraints - void setConstraints(const btContactSolverInfo& infoGlobal); + virtual void setConstraints(const btContactSolverInfo& infoGlobal); // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion virtual void predictMotion(btScalar solverdt); @@ -85,7 +93,7 @@ public: void backupVelocity(); // set m_dv and m_backupVelocity to desired value to prepare for momentum solve - void setupDeformableSolve(bool implicit); + virtual void setupDeformableSolve(bool implicit); // set the current velocity to that backed up in m_backupVelocity void revertVelocity(); @@ -150,6 +158,62 @@ public: // used in line search btScalar kineticEnergy(); + // add explicit force to the velocity in the objective class + virtual void applyExplicitForce(); + + // execute position/velocity update and apply anchor constraints in the integrateTransforms from the Dynamics world + virtual void applyTransforms(btScalar timeStep); + + virtual void setStrainLimiting(bool opt) + { + m_objective->m_projection.m_useStrainLimiting = opt; + } + + virtual void setPreconditioner(int opt) + { + switch (opt) + { + case btDeformableBackwardEulerObjective::Mass_preconditioner: + m_objective->m_preconditioner = m_objective->m_massPreconditioner; + break; + + case btDeformableBackwardEulerObjective::KKT_preconditioner: + m_objective->m_preconditioner = m_objective->m_KKTPreconditioner; + break; + + default: + btAssert(false); + break; + } + } + + virtual btAlignedObjectArray* getLagrangianForceArray() + { + return &(m_objective->m_lf); + } + + virtual const btAlignedObjectArray* getIndices() + { + return m_objective->getIndices(); + } + + virtual void setProjection() + { + m_objective->m_projection.setProjection(); + } + + virtual void setLagrangeMultiplier() + { + m_objective->m_projection.setLagrangeMultiplier(); + } + + virtual bool isReducedSolver() + { + return m_reducedSolver; + } + + virtual void deformableBodyInternalWriteBack() {} + // unused functions virtual void optimize(btAlignedObjectArray& softBodies, bool forceUpdate = false) {} virtual void solveConstraints(btScalar dt) {} diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp index 9c8e72f503f..09398d79a5c 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp @@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv { dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; } - // dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); if (!infoGlobal.m_splitImpulse) { @@ -487,9 +487,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul btVector3 dv = impulse * contact->m_c2; btSoftBody::Face* face = contact->m_face; - // save applied impulse - contact->m_cti.m_impulse = impulse; - btVector3& v0 = face->m_n[0]->m_v; btVector3& v1 = face->m_n[1]->m_v; btVector3& v2 = face->m_n[2]->m_v; diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 631fd5fbed5..0f3005417ee 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -14,11 +14,16 @@ */ #include "btDeformableMultiBodyConstraintSolver.h" +#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h" #include + // override the iterations method to include deformable/multibody contact btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { { + // pair deformable body with solver body + pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal); + ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); @@ -37,6 +42,10 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); + + // std::cout << "------------Iteration " << iteration << "------------\n"; + // std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n"; + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF @@ -51,6 +60,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_analyticsData.m_numBodies = numBodies; m_analyticsData.m_numContactManifolds = numManifolds; m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; + + m_deformableSolver->deformableBodyInternalWriteBack(); + // std::cout << "[===================Next Step===================]\n"; break; } } @@ -78,6 +90,12 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { + // reduced soft body solver directly modifies the solver body + if (m_deformableSolver->isReducedSolver()) + { + return; + } + for (int i = 0; i < numBodies; i++) { int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); @@ -94,6 +112,12 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject* void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { + // reduced soft body solver directly modifies the solver body + if (m_deformableSolver->isReducedSolver()) + { + return; + } + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; @@ -105,6 +129,53 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS } } + +void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + if (!m_deformableSolver->isReducedSolver()) + { + return; + } + + btReducedDeformableBodySolver* solver = static_cast(m_deformableSolver); + + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k) + { + btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k]; + + if (!constraint.m_contact->m_cti.m_colObj->isStaticObject()) + { + btCollisionObject& col_obj = const_cast(*constraint.m_contact->m_cti.m_colObj); + + // object index in the solver body pool + int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep); + + const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]); + if (body && body->getInvMass()) + { + // std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n"; + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + constraint.setSolverBody(bodyId, solverBody); + } + } + } + + // for (int j = 0; j < numBodies; j++) + // { + // int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep); + + // btRigidBody* body = btRigidBody::upcast(bodies[j]); + // if (body && body->getInvMass()) + // { + // btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + // m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody); + // } + // } + } +} + void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 94aabce838e..28733fa49dc 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -43,6 +43,9 @@ protected: // write the velocity of the underlying rigid body to the the the solver body void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + // let each deformable body knows which solver body is in constact + void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 983e622b5f6..030cbaf90fa 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -94,7 +94,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t beforeSolverCallbacks(timeStep); - ///solve contact constraints and then deformable bodies momemtum equation + // ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); afterSolverCallbacks(timeStep); @@ -201,7 +201,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim if (psb->isActive()) { // clear contact points in the previous iteration - psb->m_faceNodeContacts.clear(); + psb->m_faceNodeContactsCCD.clear(); // update m_q and normals for CCD calculation for (int j = 0; j < psb->m_nodes.size(); ++j) @@ -237,7 +237,8 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim btSoftBody* psb = m_softBodies[i]; if (psb->isActive()) { - penetration_count += psb->m_faceNodeContacts.size(); + penetration_count += psb->m_faceNodeContactsCCD.size(); + ; } } if (penetration_count == 0) @@ -297,83 +298,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) BT_PROFILE("integrateTransforms"); positionCorrection(timeStep); btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) - { - if (node.m_v[c] > clampDeltaV) - { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; - } - } - node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); - node.m_q = node.m_x; - node.m_vn = node.m_v; - } - // enforce anchor constraints - for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) - { - btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; - btSoftBody::Node* n = a.m_node; - n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; - - // update multibody anchor info - if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 nrm; - const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); - const btTransform& wtr = multibodyLinkCol->getWorldTransform(); - psb->m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(n->m_x), - shp, - nrm, - 0); - a.m_cti.m_normal = wtr.getBasis() * nrm; - btVector3 normal = a.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - a.m_c0 = rot.transpose() * local_impulse_matrix * rot; - a.jacobianData_normal = jacobianData_normal; - a.jacobianData_t1 = jacobianData_t1; - a.jacobianData_t2 = jacobianData_t2; - a.t1 = t1; - a.t2 = t2; - } - } - } - psb->interpolateRenderMesh(); - } + m_deformableBodySolver->applyTransforms(timeStep); } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) @@ -390,9 +315,9 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // set up the directions in which the velocity does not change in the momentum solve if (m_useProjection) - m_deformableBodySolver->m_objective->m_projection.setProjection(); + m_deformableBodySolver->setProjection(); else - m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + m_deformableBodySolver->setLagrangeMultiplier(); // for explicit scheme, m_backupVelocity = v_{n+1}^* // for implicit scheme, m_backupVelocity = v_n @@ -518,6 +443,12 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time m_deformableBodySolver->predictMotion(timeStep); } +void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity) +{ + btDiscreteDynamicsWorld::setGravity(gravity); + m_deformableBodySolver->setGravity(gravity); +} + void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { m_internalTime += timeStep; @@ -532,14 +463,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) if (m_useProjection) { m_deformableBodySolver->m_useProjection = true; - m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + m_deformableBodySolver->setStrainLimiting(true); + m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner); } else { m_deformableBodySolver->m_useProjection = false; - m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + m_deformableBodySolver->setStrainLimiting(false); + m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner); } } @@ -681,7 +612,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + btAlignedObjectArray& forces = *m_deformableBodySolver->getLagrangianForceArray(); bool added = false; for (int i = 0; i < forces.size(); ++i) { @@ -695,14 +626,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL if (!added) { force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + force->setIndices(m_deformableBodySolver->getIndices()); forces.push_back(force); } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + btAlignedObjectArray& forces = *m_deformableBodySolver->getLagrangianForceArray(); int removed_index = -1; for (int i = 0; i < forces.size(); ++i) { @@ -720,7 +651,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) { - btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + btAlignedObjectArray& forces = *m_deformableBodySolver->getLagrangianForceArray(); for (int i = 0; i < forces.size(); ++i) { forces[i]->removeSoftBody(psb); diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 4b7069aac7c..5971ecd9a92 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -19,7 +19,7 @@ #include "btSoftMultiBodyDynamicsWorld.h" #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" -#include "btDeformableBodySolver.h" +// #include "btDeformableBodySolver.h" #include "btDeformableMultiBodyConstraintSolver.h" #include "btSoftBodyHelpers.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" @@ -121,6 +121,8 @@ public: return m_sbi; } + virtual void setGravity(const btVector3& gravity); + void reinitialize(btScalar timeStep); void applyRigidBodyGravity(btScalar timeStep); diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp index c6226a008d7..e91c1b9a4aa 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp @@ -231,6 +231,9 @@ void btSoftBody::initDefaults() m_gravityFactor = 1; m_cacheBarycenter = false; m_fdbvnt = 0; + + // reduced flag + m_reducedModel = false; } // @@ -1483,6 +1486,21 @@ void btSoftBody::randomizeConstraints() #undef NEXTRAND } +void btSoftBody::updateState(const btAlignedObjectArray& q, const btAlignedObjectArray& v) +{ + int node_count = m_nodes.size(); + btAssert(node_count == q.size()); + btAssert(node_count == v.size()); + for (int i = 0; i < node_count; i++) + { + Node& n = m_nodes[i]; + n.m_x = q[i]; + n.m_q = q[i]; + n.m_v = v[i]; + n.m_vn = v[i]; + } +} + // void btSoftBody::releaseCluster(int index) { @@ -2821,7 +2839,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btScalar dst; btGjkEpaSolver2::sResults results; -// #define USE_QUADRATURE 1 + // #define USE_QUADRATURE 1 // use collision quadrature point #ifdef USE_QUADRATURE @@ -3879,7 +3897,7 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) btVector3 va(0, 0, 0); btRigidBody* rigidCol = 0; btMultiBodyLinkCollider* multibodyLinkCol = 0; - btScalar* deltaV; + btScalar* deltaV = NULL; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h index cee3ae0f23e..f91640acb6d 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h @@ -223,12 +223,10 @@ public: /* sCti is Softbody contact info */ struct sCti { - const btCollisionObject* m_colObj; /* Rigid body */ - btVector3 m_normal; /* Outward normal */ - mutable btVector3 m_impulse; /* Applied impulse */ - btScalar m_offset; /* Offset from origin */ + const btCollisionObject* m_colObj; /* Rigid body */ + btVector3 m_normal; /* Outward normal */ + btScalar m_offset; /* Offset from origin */ btVector3 m_bary; /* Barycentric weights for faces */ - sCti() : m_impulse(0, 0, 0) {} }; /* sMedium */ @@ -300,7 +298,7 @@ public: }; struct RenderFace { - RenderNode* m_n[3]; // Node pointers + RenderNode* m_n[3]; // Node pointers }; /* Face */ @@ -409,6 +407,7 @@ public: btScalar m_friction; // Friction btScalar m_imf; // inverse mass of the face at contact point btScalar m_c0; // scale of the impulse matrix; + const btCollisionObject* m_colObj; // Collision object to collide with. }; /* SContact */ @@ -788,7 +787,7 @@ public: typedef btAlignedObjectArray tClusterArray; typedef btAlignedObjectArray tNoteArray; typedef btAlignedObjectArray tNodeArray; - typedef btAlignedObjectArray< RenderNode> tRenderNodeArray; + typedef btAlignedObjectArray tRenderNodeArray; typedef btAlignedObjectArray tLeafArray; typedef btAlignedObjectArray tLinkArray; typedef btAlignedObjectArray tFaceArray; @@ -800,6 +799,7 @@ public: typedef btAlignedObjectArray tMaterialArray; typedef btAlignedObjectArray tJointArray; typedef btAlignedObjectArray tSoftBodyArray; + typedef btAlignedObjectArray > tDenseMatrix; // // Fields @@ -815,7 +815,7 @@ public: tRenderNodeArray m_renderNodes; // Render Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces - tRenderFaceArray m_renderFaces; // Faces + tRenderFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras btAlignedObjectArray m_tetraScratches; btAlignedObjectArray m_tetraScratchesTn; @@ -825,6 +825,7 @@ public: btAlignedObjectArray m_nodeRigidContacts; btAlignedObjectArray m_faceNodeContacts; btAlignedObjectArray m_faceRigidContacts; + btAlignedObjectArray m_faceNodeContactsCCD; tSContactArray m_scontacts; // Soft contacts tJointArray m_joints; // Joints tMaterialArray m_materials; // Materials @@ -857,6 +858,8 @@ public: btScalar m_restLengthScale; + bool m_reducedModel; // Reduced deformable model flag + // // Api // @@ -894,7 +897,7 @@ public: int node1) const; bool checkLink(const Node* node0, const Node* node1) const; - /* Check for existing face */ + /* Check for existring face */ bool checkFace(int node0, int node1, int node2) const; @@ -1003,15 +1006,15 @@ public: /* Get best fit rigid transform */ btTransform getRigidTransform(); /* Transform to given pose */ - void transformTo(const btTransform& trs); + virtual void transformTo(const btTransform& trs); /* Transform */ - void transform(const btTransform& trs); + virtual void transform(const btTransform& trs); /* Translate */ - void translate(const btVector3& trs); + virtual void translate(const btVector3& trs); /* Rotate */ - void rotate(const btQuaternion& rot); + virtual void rotate(const btQuaternion& rot); /* Scale */ - void scale(const btVector3& scl); + virtual void scale(const btVector3& scl); /* Get link resting lengths scale */ btScalar getRestLengthScale(); /* Scale resting length of all springs */ @@ -1053,6 +1056,9 @@ public: Material* mat = 0); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); + + void updateState(const btAlignedObjectArray& qs, const btAlignedObjectArray& vs); + /* Release clusters */ void releaseCluster(int index); void releaseClusters(); @@ -1098,6 +1104,13 @@ public: void setZeroVelocity(); bool wantsSleeping(); + virtual btMatrix3x3 getImpulseFactor(int n_node) + { + btMatrix3x3 tmp; + tmp.setIdentity(); + return tmp; + } + // // Functionality to deal with new accelerated solvers. // diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp index e464c4668dc..fbb72983060 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp @@ -18,6 +18,7 @@ subject to the following restrictions: #include #include #include +#include #include #include #include @@ -1487,6 +1488,37 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) fs.close(); } + +void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb) +{ + std::ofstream fs; + fs.open(file); + btAssert(fs); + fs << std::scientific << std::setprecision(16); + + // Only write out for trimesh, directly write out all the nodes and faces.xs + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "q"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_q[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_v[d]; + } + fs << "\n"; + } + fs.close(); +} + void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) { std::ifstream fs_read; diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h index b292d90d18b..e48d7b7513c 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h @@ -146,6 +146,11 @@ struct btSoftBodyHelpers static void writeObj(const char* file, const btSoftBody* psb); + static void writeState(const char* file, const btSoftBody* psb); + + //this code cannot be here, dependency on example code are not allowed + //static std::string loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO); + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h index e52b1c875f8..137258675b0 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h @@ -1691,12 +1691,19 @@ struct btSoftColliders if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); - // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + if (psb->m_reducedModel) + { + c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse) + } + else + { + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + } c.m_c1 = ra; } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) @@ -1724,7 +1731,16 @@ struct btSoftColliders t1.getX(), t1.getY(), t1.getZ(), t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + + btMatrix3x3 local_impulse_matrix; + if (psb->m_reducedModel) + { + local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof); + } + else + { + local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + } c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; c.jacobianData_t1 = jacobianData_t1; @@ -1944,6 +1960,7 @@ struct btSoftColliders c.m_weights = btVector3(0, 0, 0); c.m_imf = 0; c.m_c0 = 0; + c.m_colObj = psb[1]; psb[0]->m_faceNodeContacts.push_back(c); } } @@ -2019,6 +2036,7 @@ struct btSoftColliders c.m_weights = btVector3(0, 0, 0); c.m_imf = 0; c.m_c0 = 0; + c.m_colObj = psb[1]; psb[0]->m_faceNodeContacts.push_back(c); } } @@ -2050,7 +2068,8 @@ struct btSoftColliders c.m_margin = mrg; c.m_imf = 0; c.m_c0 = 0; - psb[0]->m_faceNodeContacts.push_back(c); + c.m_colObj = psb[1]; + psb[0]->m_faceNodeContactsCCD.push_back(c); } } void Process(const btDbvntNode* lface1, @@ -2114,7 +2133,8 @@ struct btSoftColliders c.m_margin = mrg; c.m_imf = 0; c.m_c0 = 0; - psb[0]->m_faceNodeContacts.push_back(c); + c.m_colObj = psb[1]; + psb[0]->m_faceNodeContactsCCD.push_back(c); } } } diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h b/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h index cdd0698494b..7eafc6c318d 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h @@ -36,7 +36,8 @@ public: CL_SIMD_SOLVER, DX_SOLVER, DX_SIMD_SOLVER, - DEFORMABLE_SOLVER + DEFORMABLE_SOLVER, + REDUCED_DEFORMABLE_SOLVER }; protected: diff --git a/thirdparty/bullet/LinearMath/btScalar.h b/thirdparty/bullet/LinearMath/btScalar.h index 28c03b2868e..9f5408c792f 100644 --- a/thirdparty/bullet/LinearMath/btScalar.h +++ b/thirdparty/bullet/LinearMath/btScalar.h @@ -25,7 +25,7 @@ subject to the following restrictions: #include /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 320 +#define BT_BULLET_VERSION 324 inline int btGetVersion() { @@ -107,7 +107,7 @@ inline int btIsDoublePrecision() #define btFsel(a,b,c) __fsel((a),(b),(c)) #else -#if defined (_M_ARM) +#if defined (_M_ARM) || defined (_M_ARM64) //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however) #elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h index 22bcac1bea5..59695ab3263 100644 --- a/thirdparty/bullet/LinearMath/btSerializer.h +++ b/thirdparty/bullet/LinearMath/btSerializer.h @@ -481,7 +481,7 @@ public: buffer[9] = '3'; buffer[10] = '2'; - buffer[11] = '1'; + buffer[11] = '4'; } virtual void startSerialization() diff --git a/thirdparty/bullet/LinearMath/btTransform.h b/thirdparty/bullet/LinearMath/btTransform.h index 310b397665d..b60c061ebc0 100644 --- a/thirdparty/bullet/LinearMath/btTransform.h +++ b/thirdparty/bullet/LinearMath/btTransform.h @@ -34,6 +34,7 @@ btTransform btVector3 m_origin; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); /**@brief No initialization constructor */ btTransform() {} /**@brief Constructor from btQuaternion (optional btVector3 ) diff --git a/thirdparty/bullet/VERSION.txt b/thirdparty/bullet/VERSION.txt index a91121ce375..bd23423169f 100644 --- a/thirdparty/bullet/VERSION.txt +++ b/thirdparty/bullet/VERSION.txt @@ -1 +1 @@ -3.21 +3.24 diff --git a/thirdparty/bullet/btBulletDynamicsAll.cpp b/thirdparty/bullet/btBulletDynamicsAll.cpp index a8069e30ae9..6e73880dc25 100644 --- a/thirdparty/bullet/btBulletDynamicsAll.cpp +++ b/thirdparty/bullet/btBulletDynamicsAll.cpp @@ -36,6 +36,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp" #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp" +#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp" #include "BulletDynamics/Vehicle/btRaycastVehicle.cpp" #include "BulletDynamics/Vehicle/btWheelInfo.cpp" #include "BulletDynamics/Character/btKinematicCharacterController.cpp" diff --git a/thirdparty/bullet/patches/fix-reorder-warning.patch b/thirdparty/bullet/patches/fix-reorder-warning.patch deleted file mode 100644 index a8e81433791..00000000000 --- a/thirdparty/bullet/patches/fix-reorder-warning.patch +++ /dev/null @@ -1,38 +0,0 @@ -diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h -index f2cc3409f9..70915366e0 100644 ---- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h -+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h -@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. --Permission is granted to anyone to use this software for any purpose, --including commercial applications, and to alter it and redistribute it freely, -+Permission is granted to anyone to use this software for any purpose, -+including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -@@ -71,9 +71,9 @@ public: - const btVector3& normal, - btScalar distance) : m_localPointA(pointA), - m_localPointB(pointB), -+ m_positionWorldOnB(0,0,0), -+ m_positionWorldOnA(0,0,0), - m_normalWorldOnB(normal), -- m_positionWorldOnB(0,0,0), -- m_positionWorldOnA(0,0,0), - m_distance1(distance), - m_combinedFriction(btScalar(0.)), - m_combinedRollingFriction(btScalar(0.)), -@@ -95,8 +95,8 @@ public: - m_contactERP(0.f), - m_frictionCFM(0.f), - m_lifeTime(0), -- m_lateralFrictionDir1(0,0,0), -- m_lateralFrictionDir2(0,0,0) -+ m_lateralFrictionDir1(0,0,0), -+ m_lateralFrictionDir2(0,0,0) - { - } -