Rémi Verschelde
parent
4a96db7196
commit
7515b47e8e
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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())
|
||||
{
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ subject to the following restrictions:
|
|||
ATTRIBUTE_ALIGNED16(class)
|
||||
btConvexHullShape : public btPolyhedralConvexAabbCachingShape
|
||||
{
|
||||
protected:
|
||||
btAlignedObjectArray<btVector3> m_unscaledPoints;
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ enum btTypedMultiBodyConstraintType
|
|||
MULTIBODY_CONSTRAINT_SLIDER,
|
||||
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
|
||||
MULTIBODY_CONSTRAINT_FIXED,
|
||||
|
||||
MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT,
|
||||
MAX_MULTIBODY_CONSTRAINT_TYPE,
|
||||
};
|
||||
|
||||
|
|
|
|||
270
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp
vendored
Normal file
270
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp
vendored
Normal file
|
|
@ -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]=0;
|
||||
damp=0;
|
||||
|
||||
} else
|
||||
{
|
||||
if (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);
|
||||
}
|
||||
}
|
||||
115
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h
vendored
Normal file
115
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h
vendored
Normal file
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
792
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp
vendored
Normal file
792
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp
vendored
Normal file
|
|
@ -0,0 +1,792 @@
|
|||
#include "btReducedDeformableBody.h"
|
||||
#include "../btSoftBodyInternals.h"
|
||||
#include "btReducedDeformableBodyHelpers.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
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;
|
||||
}
|
||||
257
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h
vendored
Normal file
257
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h
vendored
Normal file
|
|
@ -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<btVector3> TVStack;
|
||||
// typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
|
||||
typedef btAlignedObjectArray<btScalar> tDenseArray;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > 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<int> 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<int> 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
|
||||
215
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp
vendored
Normal file
215
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp
vendored
Normal file
|
|
@ -0,0 +1,215 @@
|
|||
#include "btReducedDeformableBodyHelpers.h"
|
||||
#include "../btSoftBodyHelpers.h"
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
|
||||
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<int> Index;
|
||||
std::string line;
|
||||
btAlignedObjectArray<btVector3> X;
|
||||
btVector3 position;
|
||||
btAlignedObjectArray<Index> 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<btScalar> 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));
|
||||
}
|
||||
25
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h
vendored
Normal file
25
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h
vendored
Normal file
|
|
@ -0,0 +1,25 @@
|
|||
#ifndef BT_REDUCED_SOFT_BODY_HELPERS_H
|
||||
#define BT_REDUCED_SOFT_BODY_HELPERS_H
|
||||
|
||||
#include "btReducedDeformableBody.h"
|
||||
#include <string>
|
||||
|
||||
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
|
||||
325
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp
vendored
Normal file
325
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp
vendored
Normal file
|
|
@ -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<btSoftBody*>& 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<btReducedDeformableBody*>(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<btReducedDeformableBody*>(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<btReducedDeformableBody*>(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<btReducedDeformableBody*>(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<btReducedDeformableBody*>(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<btReducedDeformableBody*>(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<int> m_orderNonContactConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderContactConstraintPool;
|
||||
|
||||
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(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<btReducedDeformableBody*>(m_softBodies[i]);
|
||||
rsb->applyInternalVelocityChanges();
|
||||
}
|
||||
m_ascendOrder = true;
|
||||
}
|
||||
61
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h
vendored
Normal file
61
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h
vendored
Normal file
|
|
@ -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<btAlignedObjectArray<btReducedDeformableStaticConstraint> > m_staticConstraints;
|
||||
btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
|
||||
btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableFaceRigidContactConstraint> > 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<btSoftBody*>& 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
|
||||
|
|
@ -0,0 +1,579 @@
|
|||
#include "btReducedDeformableContactConstraint.h"
|
||||
#include <iostream>
|
||||
|
||||
// ================= 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)
|
||||
{
|
||||
//
|
||||
}
|
||||
194
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h
vendored
Normal file
194
thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h
vendored
Normal file
|
|
@ -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<const btSoftBody::DeformableNodeRigidContact*>(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<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
|
||||
}
|
||||
|
||||
// this calls reduced deformable body's applyFullSpaceImpulse
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
};
|
||||
|
|
@ -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<btVector3> TVStack;
|
||||
btScalar m_dt;
|
||||
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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<btSoftBody*>& softBodies, btScalar dt);
|
||||
virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& 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<btDeformableLagrangianForce*>* getLagrangianForceArray()
|
||||
{
|
||||
return &(m_objective->m_lf);
|
||||
}
|
||||
|
||||
virtual const btAlignedObjectArray<btSoftBody::Node*>* 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<btSoftBody*>& softBodies, bool forceUpdate = false) {}
|
||||
virtual void solveConstraints(btScalar dt) {}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -14,11 +14,16 @@
|
|||
*/
|
||||
|
||||
#include "btDeformableMultiBodyConstraintSolver.h"
|
||||
#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
|
||||
#include <iostream>
|
||||
|
||||
// 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<btReducedDeformableBodySolver*>(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<btCollisionObject&>(*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");
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
|
||||
btAlignedObjectArray<btDeformableLagrangianForce*>& 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<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
|
||||
btAlignedObjectArray<btDeformableLagrangianForce*>& 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<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
|
||||
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
|
||||
for (int i = 0; i < forces.size(); ++i)
|
||||
{
|
||||
forces[i]->removeSoftBody(psb);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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<btVector3>& q, const btAlignedObjectArray<btVector3>& 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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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<Cluster*> tClusterArray;
|
||||
typedef btAlignedObjectArray<Note> tNoteArray;
|
||||
typedef btAlignedObjectArray<Node> tNodeArray;
|
||||
typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
|
||||
typedef btAlignedObjectArray<RenderNode> tRenderNodeArray;
|
||||
typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
|
||||
typedef btAlignedObjectArray<Link> tLinkArray;
|
||||
typedef btAlignedObjectArray<Face> tFaceArray;
|
||||
|
|
@ -800,6 +799,7 @@ public:
|
|||
typedef btAlignedObjectArray<Material*> tMaterialArray;
|
||||
typedef btAlignedObjectArray<Joint*> tJointArray;
|
||||
typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > 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<TetraScratch> m_tetraScratches;
|
||||
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
|
||||
|
|
@ -825,6 +825,7 @@ public:
|
|||
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
|
||||
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
|
||||
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
|
||||
btAlignedObjectArray<DeformableFaceNodeContact> 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<btVector3>& qs, const btAlignedObjectArray<btVector3>& 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.
|
||||
//
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@ subject to the following restrictions:
|
|||
#include <stdio.h>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <string.h>
|
||||
#include <algorithm>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<btVector3>& qs, btAlignedObjectArray<btVector3>& 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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,7 +36,8 @@ public:
|
|||
CL_SIMD_SOLVER,
|
||||
DX_SOLVER,
|
||||
DX_SIMD_SOLVER,
|
||||
DEFORMABLE_SOLVER
|
||||
DEFORMABLE_SOLVER,
|
||||
REDUCED_DEFORMABLE_SOLVER
|
||||
};
|
||||
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ subject to the following restrictions:
|
|||
#include <float.h>
|
||||
|
||||
/* 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))
|
||||
|
||||
|
|
|
|||
|
|
@ -481,7 +481,7 @@ public:
|
|||
|
||||
buffer[9] = '3';
|
||||
buffer[10] = '2';
|
||||
buffer[11] = '1';
|
||||
buffer[11] = '4';
|
||||
}
|
||||
|
||||
virtual void startSerialization()
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ btTransform
|
|||
btVector3 m_origin;
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
/**@brief No initialization constructor */
|
||||
btTransform() {}
|
||||
/**@brief Constructor from btQuaternion (optional btVector3 )
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
3.21
|
||||
3.24
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue