AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-19 05:20:06 +00:00
Code Issues Releases Wiki Activity

now support scaling of reduced deformable object. the order of applying impulse is changeable now

Browse Source
This commit is contained in:
jingyuc 2021-10-04 11:56:29 -04:00
parent 1bee1ba4c4
commit c4da596778
6 changed files with 102 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -33,7 +33,7 @@ static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar COLLIDING_VELOCITY = 0;
static int start_mode = 6;
static int num_modes = 40;
static int num_modes = 20;
class FreeFall : public CommonDeformableBodyBase
{
@ -56,14 +56,14 @@ public:
void resetCamera()
{
// float dist = 6;
// float pitch = -20;
// float yaw = 90;
// float targetPos[3] = {0, 2, 0};
float dist = 20;
float pitch = -30;
float yaw = 125;
float targetPos[3] = {-2, 0, 2};
float dist = 10;
float pitch = -20;
float yaw = 90;
float targetPos[3] = {0, 2, 0};
// float dist = 20;
// float pitch = -30;
// float yaw = 125;
// float targetPos[3] = {-2, 0, 2};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@ -143,6 +143,7 @@ void FreeFall::initPhysics()
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
// create volumetric reduced deformable body
{
@ -150,11 +151,12 @@ void FreeFall::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
rsb->scale(btVector3(1, 1, 0.5));
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 10, 0));
// init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0));
init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 4.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(20);
@ -173,7 +175,7 @@ void FreeFall::initPhysics()
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
// add a few rigid bodies
Ctor_RbUpStack();
// Ctor_RbUpStack();
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));

19
examples/ReducedDeformableDemo/ReducedCollide.cpp
View File

@ -30,10 +30,10 @@
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static btScalar damping_beta = 0.0;
static btScalar COLLIDING_VELOCITY = 4;
static int start_mode = 6;
static int num_modes = 1;
static int num_modes = 20;
class ReducedCollide : public CommonDeformableBodyBase
{
@ -75,10 +75,10 @@ public:
void Ctor_RbUpStack()
{
// float mass = 55;
float mass = 8;
float mass = 55;
// float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
@ -205,10 +205,11 @@ void ReducedCollide::initPhysics()
// create volumetric reduced deformable body
{
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
rsb->scale(btVector3(1, 1, 1));
btTransform init_transform;
init_transform.setIdentity();
@ -216,7 +217,7 @@ void ReducedCollide::initPhysics()
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(100);
rsb->setStiffnessScale(20);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -298,9 +299,9 @@ void ReducedCollide::initPhysics()
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
// {

18
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@ -39,7 +39,7 @@
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar damping_beta = 0.001;
static int start_mode = 6;
static int num_modes = 20;
static float friction = 1.;
@ -126,14 +126,14 @@ public:
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(70);
motor->setMaxAppliedImpulse(100);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(70);
motor->setMaxAppliedImpulse(100);
}
motor->setMaxAppliedImpulse(50);
motor->setMaxAppliedImpulse(200);
}
}
dofIndex += mb->getLink(link).m_dofCount;
@ -320,15 +320,15 @@ void ReducedMotorGrasp::initPhysics()
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.05);
rsb->getCollisionShape()->setMargin(0.01);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 0.5, 0));
init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(20);
rsb->setStiffnessScale(25);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->setRigidVelocity(btVector3(0, 1, 0));
@ -359,8 +359,8 @@ void ReducedMotorGrasp::initPhysics()
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
// slider.m_minVal = -.02;
// slider.m_maxVal = .02;
slider.m_minVal = -.2;
slider.m_maxVal = .2;
slider.m_minVal = -.5;
slider.m_maxVal = .5;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}

56
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
View File

@ -8,7 +8,7 @@
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = true; //! only use rigid frame to debug
m_rigidOnly = false; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;
@ -26,7 +26,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co
m_internalDeltaAngularVelocity.setZero();
m_angularFactor.setValue(1, 1, 1);
m_linearFactor.setValue(1, 1, 1);
m_invInertiaLocal.setValue(1, 1, 1);
// m_invInertiaLocal.setValue(1, 1, 1);
m_invInertiaLocal.setIdentity();
m_mass = 0.0;
m_inverseMass = 0.0;
@ -76,18 +77,18 @@ void btReducedSoftBody::setInertiaProps(const btVector3& inertia)
{
// TODO: only support box shape now
// set local inertia
m_invInertiaLocal.setValue(
inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
// m_invInertiaLocal.setValue(
// inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
// inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
// inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
updateLocalInertiaTensorFromNodes();
// update world inertia tensor
btMatrix3x3 rotation;
rotation.setIdentity();
updateInitialInertiaTensor(rotation);
// updateInitialInertiaTensorFromNodes();
updateInertiaTensor();
m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
}
@ -367,7 +368,37 @@ void btReducedSoftBody::transform(const btTransform& trs)
void btReducedSoftBody::scale(const btVector3& scl)
{
// scale the mesh
btSoftBody::scale(scl);
{
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 btReducedSoftBody::updateRestNodalPositions()
@ -380,7 +411,7 @@ void btReducedSoftBody::updateRestNodalPositions()
}
}
void btReducedSoftBody::updateInitialInertiaTensorFromNodes()
void btReducedSoftBody::updateLocalInertiaTensorFromNodes()
{
btMatrix3x3 inertia_tensor;
inertia_tensor.setZero();
@ -396,12 +427,13 @@ void btReducedSoftBody::updateInitialInertiaTensorFromNodes()
}
}
}
m_invInertiaTensorWorldInitial = inertia_tensor.inverse();
m_invInertiaLocal = inertia_tensor.inverse();
}
void btReducedSoftBody::updateInitialInertiaTensor(const btMatrix3x3& rotation)
{
m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
// m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose();
}
void btReducedSoftBody::updateModesByRotation(const btMatrix3x3& rotation)
@ -658,7 +690,7 @@ void btReducedSoftBody::applyReducedDampingForce(const tDenseArray& reduce_vel)
{
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedForceDamping[r] = - m_dampingBeta * m_Kr[r] * reduce_vel[r];
m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r];
}
}

14
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
View File

@ -46,7 +46,8 @@ class btReducedSoftBody : public btSoftBody
btScalar m_angularDamping; // angular damping coefficient
btVector3 m_linearFactor;
btVector3 m_angularFactor;
btVector3 m_invInertiaLocal;
// btVector3 m_invInertiaLocal;
btMatrix3x3 m_invInertiaLocal;
btTransform m_rigidTransformWorld;
btMatrix3x3 m_invInertiaTensorWorldInitial;
btMatrix3x3 m_invInertiaTensorWorld;
@ -122,8 +123,15 @@ class btReducedSoftBody : public btSoftBody
//
// various internal updates
//
virtual void scale(const btVector3& scl);
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);
virtual void transformTo(const btTransform& trs)
{
btAssert(false); // not supported yet
}
virtual void translate(const btVector3& trs)
{
btAssert(false); // use transform().
@ -138,7 +146,7 @@ class btReducedSoftBody : public btSoftBody
void updateInitialInertiaTensor(const btMatrix3x3& rotation);
void updateInitialInertiaTensorFromNodes();
void updateLocalInertiaTensorFromNodes();
void updateInertiaTensor();

20
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
View File

@ -273,11 +273,10 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
// shuffle the order of applying constraint
// if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
// fixed constraint order
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
@ -287,11 +286,22 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
std::cout << m_orderContactConstraintPool[j] << '\n';
}
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)