AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2430 from xhan0619/master

Browse Source 
Configure damping coefficients for neohookean models
This commit is contained in:
erwincoumans 2019-10-19 17:35:48 -04:00 committed by GitHub
commit fe79395d39
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
35 changed files with 2426 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

236
examples/DeformableDemo/DeformableClothAnchor.cpp Normal file
View File

@ -0,0 +1,236 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. 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.
*/
#include "DeformableClothAnchor.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The DeformableClothAnchor shows contact between deformable objects and rigid objects.
class DeformableClothAnchor : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
DeformableClothAnchor(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~DeformableClothAnchor()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void DeformableClothAnchor::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a ground
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
// create a piece of cloth
{
const btScalar s = 4;
const btScalar h = 6;
const int r = 9;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s), r, r, 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(100,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0, h, -(s + 3.5)));
btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3)));
psb->appendDeformableAnchor(0, body);
psb->appendDeformableAnchor(r - 1, body);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableClothAnchor::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableClothAnchor(options.m_guiHelper);
}

19
examples/DeformableDemo/DeformableClothAnchor.h Normal file
View File

@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. 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.
*/
#ifndef DEFORMABLE_CLOTH_ANCHOR_H
#define DEFORMABLE_CLOTH_ANCHOR_H
class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options);
#endif //DEFORMABLE_CLOTH_ANCHOR_H

2
examples/DeformableDemo/DeformableContact.cpp
View File

@ -196,7 +196,7 @@ void DeformableContact::initPhysics()
getDeformableDynamicsWorld()->addForce(psb2, gravity_force2);
m_forces.push_back(gravity_force2);
}
getDeformableDynamicsWorld()->setImplicit(true);
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

1
examples/DeformableDemo/DeformableMultibody.cpp
View File

@ -145,7 +145,6 @@ void DeformableMultibody::initPhysics()
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
{
bool damping = true;
bool gyro = false;

2
examples/DeformableDemo/DeformableRigid.cpp
View File

@ -231,7 +231,7 @@ void DeformableRigid::initPhysics()
// add a few rigid bodies
Ctor_RbUpStack(1);
}
getDeformableDynamicsWorld()->setImplicit(true);
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

2
examples/DeformableDemo/DeformableSelfCollision.cpp
View File

@ -175,7 +175,7 @@ void DeformableSelfCollision::initPhysics()
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
}
getDeformableDynamicsWorld()->setImplicit(true);
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

90
examples/DeformableDemo/GraspDeformable.cpp
View File

@ -190,6 +190,7 @@ void GraspDeformable::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// build a gripper
if(1)
{
bool damping = true;
bool gyro = false;
@ -269,17 +270,18 @@ void GraspDeformable::initPhysics()
}
// create a soft block
if(1)
{
char relative_path[1024];
// b3FileUtils::findFile("banana.vtk", relative_path, 1024);
// b3FileUtils::findFile("ball.vtk", relative_path, 1024);
// b3FileUtils::findFile("paper_collision.vtk", relative_path, 1024);
// b3FileUtils::findFile("deformable_crumpled_napkin_sim.vtk", relative_path, 1024);
// b3FileUtils::findFile("single_tet.vtk", relative_path, 1024);
b3FileUtils::findFile("tube.vtk", relative_path, 1024);
// b3FileUtils::findFile("tube.vtk", relative_path, 1024);
// b3FileUtils::findFile("torus.vtk", relative_path, 1024);
// b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
// b3FileUtils::findFile("bread.vtk", relative_path, 1024);
// b3FileUtils::findFile("ditto.vtk", relative_path, 1024);
b3FileUtils::findFile("ditto.vtk", relative_path, 1024);
// b3FileUtils::findFile("boot.vtk", relative_path, 1024);
// btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
// TetraCube::getElements(),
@ -289,12 +291,13 @@ void GraspDeformable::initPhysics()
btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path);
// psb->scale(btVector3(30, 30, 30)); // for banana
// psb->scale(btVector3(.2, .2, .2));
psb->scale(btVector3(.7, .7, .7));
// psb->scale(btVector3(2, 2, 2));
psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot
// psb->scale(btVector3(.1, .1, .1)); // for ditto
// psb->translate(btVector3(.25, 2, 0.4));
psb->getCollisionShape()->setMargin(0.01);
// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot
psb->scale(btVector3(.1, .1, .1)); // for ditto
// psb->translate(btVector3(.25, 10, 0.4));
psb->getCollisionShape()->setMargin(0.0005);
psb->setMaxStress(50);
psb->setTotalMass(.01);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
@ -306,44 +309,49 @@ void GraspDeformable::initPhysics()
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(10,20, 0.01);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
// // create a piece of cloth
// {
// bool onGround = false;
// const btScalar s = .4;
// btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
// btVector3(+s, 0, -s),
// btVector3(-s, 0, +s),
// btVector3(+s, 0, +s),
// create a piece of cloth
if(0)
{
bool onGround = false;
const btScalar s = .1;
const btScalar h = 1;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
20,20,
0, true);
if (onGround)
psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
// 20,20,
// 0, true);
//
// if (onGround)
// psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
// btVector3(+s, 0, -s),
// btVector3(-s, 0, +s),
// btVector3(+s, 0, +s),
// // 20,20,
// 2,2,
// 0, true);
//
// psb->getCollisionShape()->setMargin(0.02);
// psb->generateBendingConstraints(2);
// psb->setTotalMass(.01);
// psb->setSpringStiffness(5);
// psb->setDampingCoefficient(0.05);
// psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
// psb->m_cfg.kCHR = 1; // collision hardness with rigid body
// psb->m_cfg.kDF = 1;
// psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
// getDeformableDynamicsWorld()->addSoftBody(psb);
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.2,0.02, true));
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
// }
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.005);
psb->generateBendingConstraints(2);
psb->setTotalMass(.01);
psb->setSpringStiffness(10);
psb->setDampingCoefficient(0.05);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.0,0.0, true));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.05, false));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

395
examples/DeformableDemo/MultibodyClothAnchor.cpp Normal file
View File

@ -0,0 +1,395 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. 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.
*/
#include "MultibodyClothAnchor.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The MultibodyClothAnchor shows contact between deformable objects and rigid objects.
class MultibodyClothAnchor : public CommonRigidBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
MultibodyClothAnchor(struct GUIHelperInterface* helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~MultibodyClothAnchor()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 20;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonRigidBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
btMultiBody* createMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
};
void MultibodyClothAnchor::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
// deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
btVector3 gravity = btVector3(0, -20, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a ground
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -35, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(1);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
// create a piece of cloth
{
const btScalar s = 4;
const btScalar h = 6;
const int r = 9;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s), r, r, 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
bool damping = true;
bool gyro = false;
int numLinks = 5;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(1.5, .5, .5);
btVector3 baseHalfExtents(1.5, .5, .5);
btMultiBody* mbC = createMultiBody(getDeformableDynamicsWorld(), numLinks, btVector3(s+3.5f, h, -s-0.6f), linkHalfExtents, baseHalfExtents, spherical, true);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
if (numLinks > 0)
{
btScalar q0 = 0.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
// quick hack: advance time to populate the variables in multibody
m_dynamicsWorld->stepSimulation(SIMD_EPSILON, 0);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mbC->forwardKinematics(scratch_q, scratch_m);
psb->appendDeformableAnchor(0, mbC->getLink(3).m_collider);
psb->appendDeformableAnchor(r - 1, mbC->getLink(0).m_collider);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void MultibodyClothAnchor::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
// delete forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//delete collision shapes
for (int j = 0; j < m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
btMultiBody* MultibodyClothAnchor::createMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(0, 1, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(-linkHalfExtents[0] * 2.f, 0, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(-linkHalfExtents[0], 0, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void MultibodyClothAnchor::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1+2);
col->setFriction(1);
pMultiBody->setBaseCollider(col);
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(1);
pWorld->addCollisionObject(col, 2, 1+2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options)
{
return new MultibodyClothAnchor(options.m_guiHelper);
}

19
examples/DeformableDemo/MultibodyClothAnchor.h Normal file
View File

@ -0,0 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. 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.
*/
#ifndef MULTIBODY_CLOTH_ANCHOR_H
#define MULTIBODY_CLOTH_ANCHOR_H
class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options);
#endif //MULTIBODY_CLOTH_ANCHOR_H

2
examples/DeformableDemo/PinchFriction.cpp
View File

@ -350,7 +350,7 @@ void PinchFriction::initPhysics()
getDeformableDynamicsWorld()->addForce(psb3, neohookean);
m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
// add a pair of grippers
createGrip();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

2
examples/DeformableDemo/VolumetricDeformable.cpp
View File

@ -228,7 +228,7 @@ void VolumetricDeformable::initPhysics()
m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(true);
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
// add a few rigid bodies
Ctor_RbUpStack(4);

4
examples/ExampleBrowser/CMakeLists.txt
View File

@ -371,6 +371,10 @@ SET(BulletExampleBrowser_SRCS
../DeformableDemo/DeformableRigid.h
../DeformableDemo/VolumetricDeformable.cpp
../DeformableDemo/VolumetricDeformable.h
../DeformableDemo/DeformableClothAnchor.cpp
../DeformableDemo/DeformableClothAnchor.h
../DeformableDemo/MultibodyClothAnchor.cpp
../DeformableDemo/MultibodyClothAnchor.h
../MultiBody/MultiDofDemo.cpp
../MultiBody/MultiDofDemo.h
../RigidBody/RigidBodySoftContact.cpp

4
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -53,6 +53,8 @@
#include "../DeformableDemo/VolumetricDeformable.h"
#include "../DeformableDemo/GraspDeformable.h"
#include "../DeformableDemo/DeformableContact.h"
#include "../DeformableDemo/DeformableClothAnchor.h"
#include "../DeformableDemo/MultibodyClothAnchor.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsClientExample.h"
@ -198,6 +200,8 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
// ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -348,12 +348,13 @@ B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle co
return 0;
}
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda)
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu;
command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda;
command->m_loadSoftBodyArguments.m_NeoHookeanDamping = NeoHookeanDamping;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE;
return 0;
}
@ -385,6 +386,15 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_useSelfCollision = useSelfCollision;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_SELF_COLLISION;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -632,10 +632,11 @@ extern "C"
B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda);
B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping);
B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);
B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);
B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision);
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings);

43
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1631,6 +1631,7 @@ struct PhysicsServerCommandProcessorInternalData
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld;
btDeformableBodySolver* m_deformablebodySolver;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#else
btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
btSoftBodySolver* m_softbodySolver;
@ -2790,6 +2791,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
m_data->m_dynamicsWorld->removeMultiBody(mb);
delete mb;
}
#ifndef SKIP_DEFORMABLE_BODY
for (int j = 0; j < m_data->m_lf.size(); j++)
{
btDeformableLagrangianForce* force = m_data->m_lf[j];
delete force;
}
m_data->m_lf.clear();
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{
@ -8019,7 +8028,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
const std::string& error_message_prefix = "";
std::string out_found_filename;
std::string out_found_sim_filename;
int out_type, out_sim_type;
int out_type(0), out_sim_type(0);
bool render_mesh_is_sim_mesh = true;
@ -8066,7 +8075,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false));
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false);
m_data->m_dynamicsWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
}
#endif
}
@ -8079,21 +8090,28 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
{
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda));
btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
m_data->m_dynamicsWorld->addForce(psb, corotatedForce);
m_data->m_lf.push_back(corotatedForce);
}
btScalar neohookean_mu, neohookean_lambda;
btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
{
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda));
neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping;
btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
m_data->m_dynamicsWorld->addForce(psb, neohookeanForce);
m_data->m_lf.push_back(neohookeanForce);
}
btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true));
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true);
m_data->m_dynamicsWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
}
#endif
}
@ -8112,10 +8130,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
psb->m_renderNodes.resize(0);
}
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE)
{
m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity));
}
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
m_data->m_dynamicsWorld->addForce(psb, gravityForce);
m_data->m_lf.push_back(gravityForce);
btScalar collision_hardness = 1;
psb->m_cfg.kKHR = collision_hardness;
psb->m_cfg.kCHR = collision_hardness;
@ -8146,6 +8163,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
psb->setCollisionFlags(0);
psb->setTotalMass(mass);
bool use_self_collision = false;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_SELF_COLLISION)
{
use_self_collision = loadSoftBodyArgs.m_useSelfCollision;
}
psb->setSelfCollision(use_self_collision);
#else
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_kLST = 0.5;

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -501,6 +501,7 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10,
LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11,
LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
LOAD_SOFT_BODY_SET_SELF_COLLISION = 1<<13,
};
enum EnumSimParamInternalSimFlags
@ -525,9 +526,11 @@ struct LoadSoftBodyArgs
double m_corotatedLambda;
bool m_useBendingSprings;
double m_collisionHardness;
double m_useSelfCollision;
double m_frictionCoeff;
double m_NeoHookeanMu;
double m_NeoHookeanLambda;
double m_NeoHookeanDamping;
};
struct b3LoadSoftBodyResultArgs

11
src/BulletSoftBody/btConjugateGradient.h
View File

@ -27,12 +27,12 @@ class btConjugateGradient
typedef btAlignedObjectArray<btVector3> TVStack;
TVStack r,p,z,temp;
int max_iterations;
btScalar tolerance;
btScalar tolerance_squared;
public:
btConjugateGradient(const int max_it_in)
: max_iterations(max_it_in)
{
tolerance = 1024 * std::numeric_limits<btScalar>::epsilon();
tolerance_squared = 1e-5;
}
virtual ~btConjugateGradient(){}
@ -51,8 +51,7 @@ public:
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = dot(z,r);
btScalar local_tolerance = tolerance;
if (std::sqrt(r_dot_z) <= local_tolerance) {
if (r_dot_z <= tolerance_squared) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
@ -66,7 +65,6 @@ public:
// temp = A*p
A.multiply(p, temp);
A.project(temp);
// alpha = r^T * z / (p^T * A * p)
if (dot(p,temp) < SIMD_EPSILON)
{
if (verbose)
@ -77,6 +75,7 @@ public:
}
return k;
}
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z_new / dot(p, temp);
// x += alpha * p;
multAndAddTo(alpha, p, x);
@ -86,7 +85,7 @@ public:
A.precondition(r, z);
r_dot_z = r_dot_z_new;
r_dot_z_new = dot(r,z);
if (std::sqrt(r_dot_z_new) < local_tolerance) {
if (r_dot_z_new < tolerance_squared) {
if (verbose)
{
std::cout << "ConjugateGradient iterations " << k << std::endl;

4
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -101,6 +101,10 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;

49
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -18,13 +18,13 @@
#include "btDeformableBodySolver.h"
#include "btSoftBodyInternals.h"
#include "LinearMath/btQuickprof.h"
static const int kMaxConjugateGradientIterations = 200;
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(20)
, m_cg(kMaxConjugateGradientIterations)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
, m_lineSearch(true)
, m_lineSearch(false)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
}
@ -101,6 +101,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
m_residual[j].setZero();
}
}
updateVelocity();
}
}
@ -199,7 +200,7 @@ void btDeformableBodySolver::updateDv(btScalar scale)
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
m_cg.solve(*m_objective, ddv, residual, false);
m_cg.solve(*m_objective, ddv, residual);
}
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@ -247,6 +248,11 @@ void btDeformableBodySolver::updateVelocity()
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
psb->m_maxSpeedSquared = 0;
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
// set NaN to zero;
@ -255,6 +261,7 @@ void btDeformableBodySolver::updateVelocity()
m_dv[counter].setZero();
}
psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter;
}
}
@ -266,6 +273,10 @@ void btDeformableBodySolver::updateTempPosition()
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
@ -294,13 +305,23 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (implicit)
{
if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
else
m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
}
else
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter;
}
}
@ -335,14 +356,14 @@ bool btDeformableBodySolver::updateNodes()
void btDeformableBodySolver::predictMotion(btScalar solverdt)
{
// apply explicit forces to velocity
m_objective->applyExplicitForce(m_residual);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody *psb = m_softBodies[i];
if (psb->isActive())
{
// apply explicit forces to velocity
m_objective->applyExplicitForce(m_residual);
// predict motion for collision detection
predictDeformableMotion(psb, solverdt);
}
@ -371,14 +392,26 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
psb->m_sst.velmrg = psb->m_sst.sdt * 3;
psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
/* Bounds */
psb->updateBounds();
/* Integrate */
// do not allow particles to move more than the bounding box size
btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt;
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
// apply drag
n.m_v *= (1 - psb->m_cfg.drag);
// scale velocity back
if (n.m_v.norm() > max_v)
{
n.m_v.safeNormalize();
n.m_v *= max_v;
}
n.m_q = n.m_x + n.m_v * dt;
}
/* Bounds */
psb->updateBounds();
/* Nodes */
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;

119
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -15,6 +15,125 @@
#include "btDeformableContactConstraint.h"
/* ================ Deformable Node Anchor =================== */
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
: m_anchor(&a)
, btDeformableContactConstraint(a.m_cti.m_normal)
{
}
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
: m_anchor(other.m_anchor)
, btDeformableContactConstraint(other)
{
}
btVector3 btDeformableNodeAnchorConstraint::getVa() const
{
const btSoftBody::sCti& cti = m_anchor->m_cti;
btVector3 va(0, 0, 0);
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
va = cti.m_normal * vel;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += m_anchor->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += m_anchor->t2 * vel;
}
}
}
return va;
}
btScalar btDeformableNodeAnchorConstraint::solveConstraint()
{
const btSoftBody::sCti& cti = m_anchor->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = (vb - va);
// + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
const btScalar dn = btDot(vr, cti.m_normal);
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btScalar residualSquare = dn*dn;
btVector3 impulse = m_anchor->m_c0 * vr;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyImpulse(impulse, m_anchor->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
}
}
return residualSquare;
}
btVector3 btDeformableNodeAnchorConstraint::getVb() const
{
return m_anchor->m_node->m_v;
}
void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
{
btVector3 dv = impulse * m_anchor->m_c2;
m_anchor->m_node->m_v -= dv;
}
/* ================ Deformable vs. Rigid =================== */
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
: m_contact(&c)

26
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -108,6 +108,32 @@ public:
virtual void applyImpulse(const btVector3& impulse){}
};
//
// Anchor Constraint between rigid and deformable node
class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
{
public:
const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
btDeformableNodeAnchorConstraint(){}
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
virtual ~btDeformableNodeAnchorConstraint()
{
}
virtual btScalar solveConstraint();
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;
virtual btVector3 getDv(const btSoftBody::Node* n) const
{
return btVector3(0,0,0);
}
virtual void applyImpulse(const btVector3& impulse);
};
//
// Constraint between rigid/multi body and deformable objects
class btDeformableRigidContactConstraint : public btDeformableContactConstraint

44
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -32,6 +32,14 @@ btScalar btDeformableContactProjection::update()
}
}
// anchor constraints
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
{
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
@ -51,6 +59,10 @@ void btDeformableContactProjection::setConstraints()
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
@ -61,10 +73,33 @@ void btDeformableContactProjection::setConstraints()
}
}
// set Deformable Node vs. Rigid constraint
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
// set up deformable anchors
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
// skip fixed points
if (anchor.m_node->m_im == 0)
{
continue;
}
if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL)
{
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
btDeformableNodeAnchorConstraint constraint(anchor);
m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint);
}
}
// set Deformable Node vs. Rigid constraint
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
@ -242,6 +277,10 @@ void btDeformableContactProjection::setProjection()
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
int index = psb->m_nodes[j].index;
@ -249,7 +288,7 @@ void btDeformableContactProjection::setProjection()
bool existStaticConstraint = false;
btVector3 averagedNormal(0,0,0);
btAlignedObjectArray<btVector3> normals;
if (m_staticConstraints.find(index) != NULL)
if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL)
{
existStaticConstraint = true;
hasConstraint = true;
@ -438,6 +477,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
m_staticConstraints.clear();
m_nodeAnchorConstraints.clear();
m_nodeRigidConstraints.clear();
m_faceRigidConstraints.clear();
m_deformableConstraints.clear();

2
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -36,6 +36,8 @@ public:
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// map from node index to deformable constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// map from node index to node anchor constraint
btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;

8
src/BulletSoftBody/btDeformableGravityForce.h
View File

@ -57,6 +57,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& n = psb->m_nodes[j];
@ -80,6 +84,10 @@ public:
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
const btSoftBody::Node& node = psb->m_nodes[j];

24
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@ -51,6 +51,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_links.size(); ++j)
{
const btSoftBody::Link& link = psb->m_links[j];
@ -83,6 +87,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_links.size(); ++j)
{
const btSoftBody::Link& link = psb->m_links[j];
@ -108,6 +116,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j)
{
@ -138,6 +150,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_links.size(); ++j)
{
const btSoftBody::Link& link = psb->m_links[j];
@ -160,6 +176,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
sz = btMax(sz, psb->m_nodes[j].index);
@ -188,6 +208,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
btScalar scaled_k = m_elasticStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j)
{

6
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -23,7 +23,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
m_maxOverrideNumSolverIterations = 50;
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
@ -33,10 +32,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// solver body velocity -> rigid body velocity
solverBodyWriteBack(infoGlobal);
btScalar deformableResidual = m_deformableSolver->solveContactConstraints();
// update rigid body velocity in rigid/deformable contact
m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints());
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
// solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal);

101
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -33,7 +33,7 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
#include "btSoftBodyInternals.h"
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
@ -70,26 +70,52 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
///update vehicle simulation
btMultiBodyDynamicsWorld::updateActions(timeStep);
btMultiBodyDynamicsWorld::updateActivationState(timeStep);
updateActivationState(timeStep);
// End solver-wise simulation step
// ///////////////////////////////
}
void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
{
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody* psb = m_softBodies[i];
psb->updateDeactivation(timeStep);
if (psb->wantsSleeping())
{
if (psb->getActivationState() == ACTIVE_TAG)
psb->setActivationState(WANTS_DEACTIVATION);
if (psb->getActivationState() == ISLAND_SLEEPING)
{
psb->setZeroVelocity();
}
}
else
{
if (psb->getActivationState() != DISABLE_DEACTIVATION)
psb->setActivationState(ACTIVE_TAG);
}
}
btMultiBodyDynamicsWorld::updateActivationState(timeStep);
}
void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
{
m_deformableBodySolver->updateSoftBodies();
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody* psb = (btSoftBody*)m_softBodies[i];
btSoftBody* psb = m_softBodies[i];
if (psb->isActive())
{
psb->defaultCollisionHandler(psb);
}
}
}
void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
//m_deformableBodySolver->backupVelocity();
//positionCorrection(timeStep); // looks like position correction is no longer necessary
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
for (int i = 0; i < m_softBodies.size(); ++i)
{
@ -114,18 +140,66 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
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->revertVelocity();
}
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
{
if (!m_implicit)
{
// save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver->backupVelocity();
}
// set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints();
@ -386,3 +460,12 @@ void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
// force a reinitialize so that node indices get updated.
m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
}
void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
{
btSoftBody* body = btSoftBody::upcast(collisionObject);
if (body)
removeSoftBody(body);
else
btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
}

7
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@ -62,6 +62,8 @@ protected:
void solveConstraints(btScalar timeStep);
void updateActivationState(btScalar timeStep);
public:
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
@ -83,7 +85,8 @@ public:
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = true;
m_implicit = false;
m_lineSearch = false;
m_selfCollision = true;
}
@ -147,6 +150,8 @@ public:
void removeSoftBody(btSoftBody* body);
void removeCollisionObject(btCollisionObject* collisionObject);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }

59
src/BulletSoftBody/btDeformableNeoHookeanForce.h
View File

@ -18,6 +18,7 @@ subject to the following restrictions:
#include "btDeformableLagrangianForce.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btImplicitQRSVD.h"
// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
{
@ -32,7 +33,7 @@ public:
m_lambda_damp = damping * m_lambda;
}
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda)
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
@ -60,6 +61,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
@ -72,8 +77,10 @@ public:
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
btMatrix3x3 I;
I.setIdentity();
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
@ -93,6 +100,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
@ -111,6 +122,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
sz = btMax(sz, psb->m_nodes[j].index);
@ -150,11 +165,35 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
btScalar max_p = psb->m_cfg.m_maxStress;
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btMatrix3x3 P;
firstPiola(psb->m_tetraScratches[j],P);
btMatrix3x3 U, V;
btVector3 sigma;
singularValueDecomposition(P, U, sigma, V);
if (max_p > 0)
{
sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p);
}
btMatrix3x3 Sigma;
Sigma.setIdentity();
Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose();
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
@ -189,6 +228,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
@ -201,8 +244,10 @@ public:
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
btMatrix3x3 I;
I.setIdentity();
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
@ -225,6 +270,10 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];

242
src/BulletSoftBody/btSoftBody.cpp
View File

@ -89,6 +89,8 @@ void btSoftBody::initDefaults()
m_cfg.piterations = 1;
m_cfg.diterations = 0;
m_cfg.citerations = 4;
m_cfg.drag = 0;
m_cfg.m_maxStress = 0;
m_cfg.collisions = fCollision::Default;
m_cfg.collisions |= fCollision::VF_DD;
m_pose.m_bvolume = false;
@ -115,6 +117,9 @@ void btSoftBody::initDefaults()
m_windVelocity = btVector3(0, 0, 0);
m_restLengthScale = btScalar(1.0);
m_dampingCoefficient = 1;
m_sleepingThreshold = 0.1;
m_useFaceContact = false;
m_collisionFlags = 0;
}
//
@ -406,6 +411,98 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca
m_anchors.push_back(a);
}
//
void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
{
DeformableNodeRigidAnchor c;
btSoftBody::Node& n = m_nodes[node];
const btScalar ima = n.m_im;
const btScalar imb = body->getInvMass();
btVector3 nrm;
const btCollisionShape* shp = body->getCollisionShape();
const btTransform& wtr = body->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(m_nodes[node].m_x),
shp,
nrm,
0);
c.m_cti.m_colObj = body;
c.m_cti.m_normal = wtr.getBasis() * nrm;
c.m_cti.m_offset = dst;
c.m_node = &m_nodes[node];
const btScalar fc = m_cfg.kDF * body->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
c.m_node->m_battach = 1;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
{
DeformableNodeRigidAnchor c;
btSoftBody::Node& n = m_nodes[node];
const btScalar ima = n.m_im;
btVector3 nrm;
const btCollisionShape* shp = link->getCollisionShape();
const btTransform& wtr = link->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(m_nodes[node].m_x),
shp,
nrm,
0);
c.m_cti.m_colObj = link;
c.m_cti.m_normal = wtr.getBasis() * nrm;
c.m_cti.m_offset = dst;
c.m_node = &m_nodes[node];
const btScalar fc = m_cfg.kDF * link->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
btVector3 normal = c.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
findJacobian(link, jacobianData_t2, c.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 = link->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();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c1 = ra;
c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
c.m_node->m_battach = 1;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
{
@ -2306,10 +2403,10 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
//const btTransform& wtr = colObjWrap->getWorldTransform();
// btTransform wtr = (predict) ?
// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
// : colObjWrap->getWorldTransform();
const btTransform& wtr = colObjWrap->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(x),
@ -2427,15 +2524,48 @@ void btSoftBody::updateBounds()
m_bounds[1] = btVector3(1000, 1000, 1000);
} else {*/
if (m_ndbvt.m_root)
// if (m_ndbvt.m_root)
// {
// const btVector3& mins = m_ndbvt.m_root->volume.Mins();
// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
// const btScalar csm = getCollisionShape()->getMargin();
// const btVector3 mrg = btVector3(csm,
// csm,
// csm) *
// 1; // ??? to investigate...
// m_bounds[0] = mins - mrg;
// m_bounds[1] = maxs + mrg;
// if (0 != getBroadphaseHandle())
// {
// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
// m_bounds[0],
// m_bounds[1],
// m_worldInfo->m_dispatcher);
// }
// }
// else
// {
// m_bounds[0] =
// m_bounds[1] = btVector3(0, 0, 0);
// }
if (m_nodes.size())
{
const btVector3& mins = m_ndbvt.m_root->volume.Mins();
const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
btVector3 mins = m_nodes[0].m_x;
btVector3 maxs = m_nodes[0].m_x;
for (int i = 1; i < m_nodes.size(); ++i)
{
for (int d = 0; d < 3; ++d)
{
if (m_nodes[i].m_x[d] > maxs[d])
maxs[d] = m_nodes[i].m_x[d];
if (m_nodes[i].m_x[d] < mins[d])
mins[d] = m_nodes[i].m_x[d];
}
}
const btScalar csm = getCollisionShape()->getMargin();
const btVector3 mrg = btVector3(csm,
csm,
csm) *
1; // ??? to investigate...
csm);
m_bounds[0] = mins - mrg;
m_bounds[1] = maxs + mrg;
if (0 != getBroadphaseHandle())
@ -2451,7 +2581,6 @@ void btSoftBody::updateBounds()
m_bounds[0] =
m_bounds[1] = btVector3(0, 0, 0);
}
//}
}
//
@ -3162,6 +3291,12 @@ void btSoftBody::applyForces()
}
}
//
void btSoftBody::setMaxStress(btScalar maxStress)
{
m_cfg.m_maxStress = maxStress;
}
//
void btSoftBody::interpolateRenderMesh()
{
@ -3378,6 +3513,16 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
return (0);
}
void btSoftBody::setSelfCollision(bool useSelfCollision)
{
m_useSelfCollision = useSelfCollision;
}
bool btSoftBody::useSelfCollision()
{
return m_useSelfCollision;
}
//
void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
{
@ -3420,16 +3565,18 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
{
btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
btTransform wtr = pcoWrap->getWorldTransform();
const btTransform ctr = pcoWrap->getWorldTransform();
const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
{
const btTransform wtr = pcoWrap->getWorldTransform();
// const btTransform ctr = pcoWrap->getWorldTransform();
// const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
const btScalar timemargin = 0;
const btScalar basemargin = getCollisionShape()->getMargin();
btVector3 mins;
btVector3 maxs;
ATTRIBUTE_ALIGNED16(btDbvtVolume)
volume;
pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
pcoWrap->getCollisionShape()->getAabb(wtr,
mins,
maxs);
volume = btDbvtVolume::FromMM(mins, maxs);
@ -3442,6 +3589,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
docollideNode.stamargin = basemargin;
m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
if (this->m_useFaceContact)
{
btSoftColliders::CollideSDF_RDF docollideFace;
docollideFace.psb = this;
docollideFace.m_colObj1Wrap = pcoWrap;
@ -3450,6 +3599,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
docollideFace.stamargin = basemargin;
m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
}
}
}
break;
}
}
@ -3541,6 +3692,8 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
}
break;
case fCollision::VF_DD:
{
if (psb->isActive() || this->isActive())
{
if (this != psb)
{
@ -3562,6 +3715,8 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
docollide);
}
else
{
if (psb->useSelfCollision())
{
btSoftColliders::CollideFF_DD docollide;
docollide.mrg = getCollisionShape()->getMargin() +
@ -3573,17 +3728,8 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
calculateNormalCone(root);
this->m_fdbvt.selfCollideT(root,docollide);
delete root;
// btSoftColliders::CollideFF_DD docollide;
// /* common */
// docollide.mrg = getCollisionShape()->getMargin() +
// psb->getCollisionShape()->getMargin();
// /* psb0 nodes vs psb1 faces */
// docollide.psb[0] = this;
// docollide.psb[1] = psb;
// docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root,
// docollide.psb[1]->m_fdbvt.m_root,
// docollide);
}
}
}
}
break;
@ -3989,3 +4135,47 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
return btSoftBodyDataName;
}
void btSoftBody::updateDeactivation(btScalar timeStep)
{
if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
return;
if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
{
m_deactivationTime += timeStep;
}
else
{
m_deactivationTime = btScalar(0.);
setActivationState(0);
}
}
void btSoftBody::setZeroVelocity()
{
for (int i = 0; i < m_nodes.size(); ++i)
{
m_nodes[i].m_v.setZero();
}
}
bool btSoftBody::wantsSleeping()
{
if (getActivationState() == DISABLE_DEACTIVATION)
return false;
//disable deactivation
if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
return false;
if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
return true;
if (m_deactivationTime > gDeactivationTime)
{
return true;
}
return false;
}

26
src/BulletSoftBody/btSoftBody.h
View File

@ -354,6 +354,12 @@ public:
Node* m_node; // Owner node
};
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
{
public:
btVector3 m_local; // Anchor position in body space
};
class DeformableFaceRigidContact : public DeformableRigidContact
{
public:
@ -702,6 +708,8 @@ public:
tVSolverArray m_vsequence; // Velocity solvers sequence
tPSolverArray m_psequence; // Position solvers sequence
tPSolverArray m_dsequence; // Drift solvers sequence
btScalar drag; // deformable air drag
btScalar m_maxStress; // Maximum principle first Piola stress
};
/* SolverState */
struct SolverState
@ -772,6 +780,7 @@ public:
btAlignedObjectArray<TetraScratch> m_tetraScratches;
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
tAnchorArray m_anchors; // Anchors
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
@ -787,9 +796,13 @@ public:
btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters
btScalar m_dampingCoefficient; // Damping Coefficient
btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared;
bool m_useFaceContact;
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
bool m_useSelfCollision;
btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
@ -827,6 +840,11 @@ public:
m_dampingCoefficient = damping_coeff;
}
void setUseFaceContact(bool useFaceContact)
{
m_useFaceContact = false;
}
///@todo: avoid internal softbody shape hack and move collision code to collision library
virtual void setCollisionShape(btCollisionShape* collisionShape)
{
@ -886,6 +904,8 @@ public:
Material* mat = 0);
/* Append anchor */
void appendDeformableAnchor(int node, btRigidBody* body);
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
void appendAnchor(int node,
btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
@ -1006,6 +1026,11 @@ public:
/* defaultCollisionHandlers */
void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
void defaultCollisionHandler(btSoftBody* psb);
void setSelfCollision(bool useSelfCollision);
bool useSelfCollision();
void updateDeactivation(btScalar timeStep);
void setZeroVelocity();
bool wantsSleeping();
//
// Functionality to deal with new accelerated solvers.
@ -1103,6 +1128,7 @@ public:
void updateDeformation();
void advanceDeformation();
void applyForces();
void setMaxStress(btScalar maxStress);
void interpolateRenderMesh();
static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);

6
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -1074,6 +1074,7 @@ struct btSoftColliders
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
{
const btScalar ima = n.m_im;
// todo: collision between multibody and fixed deformable node will be missed.
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
@ -1096,8 +1097,6 @@ struct btSoftColliders
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
if (m_rigidBody)
m_rigidBody->activate();
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
@ -1169,6 +1168,7 @@ struct btSoftColliders
{
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
// todo: collision between multibody and fixed deformable face will be missed.
const btScalar ms = ima + imb;
if (ms > 0)
{
@ -1198,8 +1198,6 @@ struct btSoftColliders
// we do not scale the impulse matrix by dt
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
if (m_rigidBody)
m_rigidBody->activate();
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{

1
src/LinearMath/CMakeLists.txt
View File

@ -32,6 +32,7 @@ SET(LinearMath_HDRS
btIDebugDraw.h
btList.h
btMatrix3x3.h
btImplicitQRSVD.h
btMinMax.h
btMotionState.h
btPolarDecomposition.h

889
src/LinearMath/btImplicitQRSVD.h Normal file
View File

@ -0,0 +1,889 @@
/**
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. 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.
Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
If the code is used in an article, the following paper shall be cited:
@techreport{qrsvd:2016,
title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
year={2016},
institution={University of California Los Angeles}
}
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
**/
#ifndef btImplicitQRSVD_h
#define btImplicitQRSVD_h
#include "btMatrix3x3.h"
class btMatrix2x2
{
public:
btScalar m_00, m_01, m_10, m_11;
btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
{
}
btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11)
{}
btScalar& operator()(int i, int j)
{
if (i == 0 && j == 0)
return m_00;
if (i == 1 && j == 0)
return m_10;
if (i == 0 && j == 1)
return m_01;
if (i == 1 && j == 1)
return m_11;
btAssert(false);
return m_00;
}
const btScalar& operator()(int i, int j) const
{
if (i == 0 && j == 0)
return m_00;
if (i == 1 && j == 0)
return m_10;
if (i == 0 && j == 1)
return m_01;
if (i == 1 && j == 1)
return m_11;
btAssert(false);
return m_00;
}
void setIdentity()
{
m_00 = 1;
m_11 = 1;
m_01 = 0;
m_10 = 0;
}
};
static inline btScalar copySign(btScalar x, btScalar y) {
if ((x < 0 && y > 0) || (x > 0 && y < 0))
return -x;
return x;
}
/**
Class for givens rotation.
Row rotation G*A corresponds to something like
c -s 0
( s c 0 ) A
0 0 1
Column rotation A G' corresponds to something like
c -s 0
A ( s c 0 )
0 0 1
c and s are always computed so that
( c -s ) ( a ) = ( * )
s c b ( 0 )
Assume rowi<rowk.
*/
class GivensRotation {
public:
int rowi;
int rowk;
btScalar c;
btScalar s;
inline GivensRotation(int rowi_in, int rowk_in)
: rowi(rowi_in)
, rowk(rowk_in)
, c(1)
, s(0)
{
}
inline GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
: rowi(rowi_in)
, rowk(rowk_in)
{
compute(a, b);
}
~GivensRotation() {}
inline void transposeInPlace()
{
s = -s;
}
/**
Compute c and s from a and b so that
( c -s ) ( a ) = ( * )
s c b ( 0 )
*/
inline void compute(const btScalar a, const btScalar b)
{
btScalar d = a * a + b * b;
c = 1;
s = 0;
if (d != 0) {
btScalar t = btScalar(1.0)/btSqrt(d);
c = a * t;
s = -b * t;
}
}
/**
This function computes c and s so that
( c -s ) ( a ) = ( 0 )
s c b ( * )
*/
inline void computeUnconventional(const btScalar a, const btScalar b)
{
btScalar d = a * a + b * b;
c = 0;
s = 1;
if (d != 0) {
btScalar t = btScalar(1.0)/btSqrt(d);
s = a * t;
c = b * t;
}
}
/**
Fill the R with the entries of this rotation
*/
inline void fill(const btMatrix3x3& R) const
{
btMatrix3x3& A = const_cast<btMatrix3x3&>(R);
A.setIdentity();
A[rowi][rowi] = c;
A[rowk][rowi] = -s;
A[rowi][rowk] = s;
A[rowk][rowk] = c;
}
inline void fill(const btMatrix2x2& R) const
{
btMatrix2x2& A = const_cast<btMatrix2x2&>(R);
A(rowi,rowi) = c;
A(rowk,rowi) = -s;
A(rowi,rowk) = s;
A(rowk,rowk) = c;
}
/**
This function does something like
c -s 0
( s c 0 ) A -> A
0 0 1
It only affects row i and row k of A.
*/
inline void rowRotation(btMatrix3x3& A) const
{
for (int j = 0; j < 3; j++) {
btScalar tau1 = A[rowi][j];
btScalar tau2 = A[rowk][j];
A[rowi][j] = c * tau1 - s * tau2;
A[rowk][j] = s * tau1 + c * tau2;
}
}
inline void rowRotation(btMatrix2x2& A) const
{
for (int j = 0; j < 2; j++) {
btScalar tau1 = A(rowi,j);
btScalar tau2 = A(rowk,j);
A(rowi,j) = c * tau1 - s * tau2;
A(rowk,j) = s * tau1 + c * tau2;
}
}
/**
This function does something like
c s 0
A ( -s c 0 ) -> A
0 0 1
It only affects column i and column k of A.
*/
inline void columnRotation(btMatrix3x3& A) const
{
for (int j = 0; j < 3; j++) {
btScalar tau1 = A[j][rowi];
btScalar tau2 = A[j][rowk];
A[j][rowi] = c * tau1 - s * tau2;
A[j][rowk] = s * tau1 + c * tau2;
}
}
inline void columnRotation(btMatrix2x2& A) const
{
for (int j = 0; j < 2; j++) {
btScalar tau1 = A(j,rowi);
btScalar tau2 = A(j,rowk);
A(j,rowi) = c * tau1 - s * tau2;
A(j,rowk) = s * tau1 + c * tau2;
}
}
/**
Multiply givens must be for same row and column
**/
inline void operator*=(const GivensRotation& A)
{
btScalar new_c = c * A.c - s * A.s;
btScalar new_s = s * A.c + c * A.s;
c = new_c;
s = new_s;
}
/**
Multiply givens must be for same row and column
**/
inline GivensRotation operator*(const GivensRotation& A) const
{
GivensRotation r(*this);
r *= A;
return r;
}
};
/**
\brief zero chasing the 3X3 matrix to bidiagonal form
original form of H: x x 0
x x x
0 0 x
after zero chase:
x x 0
0 x x
0 0 x
*/
inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
{
/**
Reduce H to of form
x x +
0 x x
0 0 x
*/
GivensRotation r1(H[0][0], H[1][0], 0, 1);
/**
Reduce H to of form
x x 0
0 x x
0 + x
Can calculate r2 without multiplying by r1 since both entries are in first two
rows thus no need to divide by sqrt(a^2+b^2)
*/
GivensRotation r2(1, 2);
if (H[1][0] != 0)
r2.compute(H[0][0] * H[0][1] + H[1][0] * H[1][1], H[0][0] * H[0][2] + H[1][0] * H[1][2]);
else
r2.compute(H[0][1], H[0][2]);
r1.rowRotation(H);
/* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
r2.columnRotation(H);
r2.columnRotation(V);
/**
Reduce H to of form
x x 0
0 x x
0 0 x
*/
GivensRotation r3(H[1][1], H[2][1], 1, 2);
r3.rowRotation(H);
// Save this till end for better cache coherency
// r1.rowRotation(u_transpose);
// r3.rowRotation(u_transpose);
r1.columnRotation(U);
r3.columnRotation(U);
}
/**
\brief make a 3X3 matrix to upper bidiagonal form
original form of H: x x x
x x x
x x x
after zero chase:
x x 0
0 x x
0 0 x
*/
inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
{
U.setIdentity();
V.setIdentity();
/**
Reduce H to of form
x x x
x x x
0 x x
*/
GivensRotation r(H[1][0], H[2][0], 1, 2);
r.rowRotation(H);
// r.rowRotation(u_transpose);
r.columnRotation(U);
// zeroChase(H, u_transpose, V);
zeroChase(H, U, V);
}
/**
\brief make a 3X3 matrix to lambda shape
original form of H: x x x
* x x x
* x x x
after :
* x 0 0
* x x 0
* x 0 x
*/
inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
{
U.setIdentity();
V.setIdentity();
/**
Reduce H to of form
* x x 0
* x x x
* x x x
*/
GivensRotation r1(H[0][1], H[0][2], 1, 2);
r1.columnRotation(H);
r1.columnRotation(V);
/**
Reduce H to of form
* x x 0
* x x 0
* x x x
*/
r1.computeUnconventional(H[1][2], H[2][2]);
r1.rowRotation(H);
r1.columnRotation(U);
/**
Reduce H to of form
* x x 0
* x x 0
* x 0 x
*/
GivensRotation r2(H[2][0], H[2][1], 0, 1);
r2.columnRotation(H);
r2.columnRotation(V);
/**
Reduce H to of form
* x 0 0
* x x 0
* x 0 x
*/
r2.computeUnconventional(H[0][1], H[1][1]);
r2.rowRotation(H);
r2.columnRotation(U);
}
/**
\brief 2x2 polar decomposition.
\param[in] A matrix.
\param[out] R Robustly a rotation matrix.
\param[out] S_Sym Symmetric. Whole matrix is stored
Polar guarantees negative sign is on the small magnitude singular value.
S is guaranteed to be the closest one to identity.
R is guaranteed to be the closest rotation to A.
*/
inline void polarDecomposition(const btMatrix2x2& A,
GivensRotation& R,
const btMatrix2x2& S_Sym)
{
btScalar a = (A(0, 0) + A(1, 1)), b = (A(1, 0) - A(0, 1));
btScalar denominator = btSqrt(a*a+b*b);
R.c = (btScalar)1;
R.s = (btScalar)0;
if (denominator != 0) {
/*
No need to use a tolerance here because x(0) and x(1) always have
smaller magnitude then denominator, therefore overflow never happens.
*/
R.c = a / denominator;
R.s = -b / denominator;
}
btMatrix2x2& S = const_cast<btMatrix2x2&>(S_Sym);
S = A;
R.rowRotation(S);
}
inline void polarDecomposition(const btMatrix2x2& A,
const btMatrix2x2& R,
const btMatrix2x2& S_Sym)
{
GivensRotation r(0, 1);
polarDecomposition(A, r, S_Sym);
r.fill(R);
}
/**
\brief 2x2 SVD (singular value decomposition) A=USV'
\param[in] A Input matrix.
\param[out] U Robustly a rotation matrix in Givens form
\param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative.
\param[out] V Robustly a rotation matrix in Givens form
*/
inline void singularValueDecomposition(
const btMatrix2x2& A,
GivensRotation& U,
const btMatrix2x2& Sigma,
GivensRotation& V,
const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
{
btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
sigma.setIdentity();
btMatrix2x2 S_Sym;
polarDecomposition(A, U, S_Sym);
btScalar cosine, sine;
btScalar x = S_Sym(0, 0);
btScalar y = S_Sym(0, 1);
btScalar z = S_Sym(1, 1);
if (y == 0) {
// S is already diagonal
cosine = 1;
sine = 0;
sigma(0,0) = x;
sigma(1,1) = z;
}
else {
btScalar tau = 0.5 * (x - z);
btScalar w = btSqrt(tau * tau + y * y);
// w > y > 0
btScalar t;
if (tau > 0) {
// tau + w > w > y > 0 ==> division is safe
t = y / (tau + w);
}
else {
// tau - w < -w < -y < 0 ==> division is safe
t = y / (tau - w);
}
cosine = btScalar(1) / btSqrt(t * t + btScalar(1));
sine = -t * cosine;
/*
V = [cosine -sine; sine cosine]
Sigma = V'SV. Only compute the diagonals for efficiency.
Also utilize symmetry of S and don't form V yet.
*/
btScalar c2 = cosine * cosine;
btScalar csy = 2 * cosine * sine * y;
btScalar s2 = sine * sine;
sigma(0,0) = c2 * x - csy + s2 * z;
sigma(1,1) = s2 * x + csy + c2 * z;
}
// Sorting
// Polar already guarantees negative sign is on the small magnitude singular value.
if (sigma(0,0) < sigma(1,1)) {
std::swap(sigma(0,0), sigma(1,1));
V.c = -sine;
V.s = cosine;
}
else {
V.c = cosine;
V.s = sine;
}
U *= V;
}
/**
\brief 2x2 SVD (singular value decomposition) A=USV'
\param[in] A Input matrix.
\param[out] U Robustly a rotation matrix.
\param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
\param[out] V Robustly a rotation matrix.
*/
inline void singularValueDecomposition(
const btMatrix2x2& A,
const btMatrix2x2& U,
const btMatrix2x2& Sigma,
const btMatrix2x2& V,
const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
{
GivensRotation gv(0, 1);
GivensRotation gu(0, 1);
singularValueDecomposition(A, gu, Sigma, gv);
gu.fill(U);
gv.fill(V);
}
/**
\brief compute wilkinsonShift of the block
a1 b1
b1 a2
based on the wilkinsonShift formula
mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
*/
inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
{
btScalar d = (btScalar)0.5 * (a1 - a2);
btScalar bs = b1 * b1;
btScalar mu = a2 - copysign(bs / (btFabs(d) + btSqrt(d * d + bs)), d);
// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
return mu;
}
/**
\brief Helper function of 3X3 SVD for processing 2X2 SVD
*/
template <int t>
inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V)
{
int other = (t == 1) ? 0 : 2;
GivensRotation u(0, 1);
GivensRotation v(0, 1);
sigma[other] = B[other][other];
btMatrix2x2 B_sub, sigma_sub;
if (t == 0)
{
B_sub.m_00 = B[0][0];
B_sub.m_10 = B[1][0];
B_sub.m_01 = B[0][1];
B_sub.m_11 = B[1][1];
sigma_sub.m_00 = sigma[0];
sigma_sub.m_11 = sigma[1];
// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
singularValueDecomposition(B_sub, u, sigma_sub, v);
B[0][0] = B_sub.m_00;
B[1][0] = B_sub.m_10;
B[0][1] = B_sub.m_01;
B[1][1] = B_sub.m_11;
sigma[0] = sigma_sub.m_00;
sigma[1] = sigma_sub.m_11;
}
else
{
B_sub.m_00 = B[1][1];
B_sub.m_10 = B[2][1];
B_sub.m_01 = B[1][2];
B_sub.m_11 = B[2][2];
sigma_sub.m_00 = sigma[1];
sigma_sub.m_11 = sigma[2];
// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
singularValueDecomposition(B_sub, u, sigma_sub, v);
B[1][1] = B_sub.m_00;
B[2][1] = B_sub.m_10;
B[1][2] = B_sub.m_01;
B[2][2] = B_sub.m_11;
sigma[1] = sigma_sub.m_00;
sigma[2] = sigma_sub.m_11;
}
u.rowi += t;
u.rowk += t;
v.rowi += t;
v.rowk += t;
u.columnRotation(U);
v.columnRotation(V);
}
/**
\brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
*/
inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma)
{
sigma[i] = -sigma[i];
U[0][i] = -U[0][i];
U[1][i] = -U[1][i];
U[2][i] = -U[2][i];
}
inline void flipSign(int i, btMatrix3x3& U)
{
U[0][i] = -U[0][i];
U[1][i] = -U[1][i];
U[2][i] = -U[2][i];
}
inline void swapCol(btMatrix3x3& A, int i, int j)
{
for (int d = 0; d < 3; ++d)
std::swap(A[d][i], A[d][j]);
}
/**
\brief Helper function of 3X3 SVD for sorting singular values
*/
inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t)
{
if (t == 0)
{
// Case: sigma(0) > |sigma(1)| >= |sigma(2)|
if (btFabs(sigma[1]) >= btFabs(sigma[2])) {
if (sigma[1] < 0) {
flipSign(1, U, sigma);
flipSign(2, U, sigma);
}
return;
}
//fix sign of sigma for both cases
if (sigma[2] < 0) {
flipSign(1, U, sigma);
flipSign(2, U, sigma);
}
//swap sigma(1) and sigma(2) for both cases
std::swap(sigma[1], sigma[2]);
// swap the col 1 and col 2 for U,V
swapCol(U,1,2);
swapCol(V,1,2);
// Case: |sigma(2)| >= sigma(0) > |simga(1)|
if (sigma[1] > sigma[0]) {
std::swap(sigma[0], sigma[1]);
swapCol(U,0,1);
swapCol(V,0,1);
}
// Case: sigma(0) >= |sigma(2)| > |simga(1)|
else {
flipSign(2, U);
flipSign(2, V);
}
}
else if (t == 1)
{
// Case: |sigma(0)| >= sigma(1) > |sigma(2)|
if (btFabs(sigma[0]) >= sigma[1]) {
if (sigma[0] < 0) {
flipSign(0, U, sigma);
flipSign(2, U, sigma);
}
return;
}
//swap sigma(0) and sigma(1) for both cases
std::swap(sigma[0], sigma[1]);
swapCol(U, 0, 1);
swapCol(V, 0, 1);
// Case: sigma(1) > |sigma(2)| >= |sigma(0)|
if (btFabs(sigma[1]) < btFabs(sigma[2])) {
std::swap(sigma[1], sigma[2]);
swapCol(U, 1, 2);
swapCol(V, 1, 2);
}
// Case: sigma(1) >= |sigma(0)| > |sigma(2)|
else {
flipSign(1, U);
flipSign(1, V);
}
// fix sign for both cases
if (sigma[1] < 0) {
flipSign(1, U, sigma);
flipSign(2, U, sigma);
}
}
}
/**
\brief 3X3 SVD (singular value decomposition) A=USV'
\param[in] A Input matrix.
\param[out] U is a rotation matrix.
\param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
\param[out] V is a rotation matrix.
*/
inline int singularValueDecomposition(const btMatrix3x3& A,
btMatrix3x3& U,
btVector3& sigma,
btMatrix3x3& V,
btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
{
using std::fabs;
btMatrix3x3 B = A;
U.setIdentity();
V.setIdentity();
makeUpperBidiag(B, U, V);
int count = 0;
btScalar mu = (btScalar)0;
GivensRotation r(0, 1);
btScalar alpha_1 = B[0][0];
btScalar beta_1 = B[0][1];
btScalar alpha_2 = B[1][1];
btScalar alpha_3 = B[2][2];
btScalar beta_2 = B[1][2];
btScalar gamma_1 = alpha_1 * beta_1;
btScalar gamma_2 = alpha_2 * beta_2;
tol *= btMax((btScalar)0.5 * btSqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1);
/**
Do implicit shift QR until A^T A is block diagonal
*/
while (btFabs(beta_2) > tol && btFabs(beta_1) > tol
&& btFabs(alpha_1) > tol && btFabs(alpha_2) > tol
&& btFabs(alpha_3) > tol) {
mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
r.compute(alpha_1 * alpha_1 - mu, gamma_1);
r.columnRotation(B);
r.columnRotation(V);
zeroChase(B, U, V);
alpha_1 = B[0][0];
beta_1 = B[0][1];
alpha_2 = B[1][1];
alpha_3 = B[2][2];
beta_2 = B[1][2];
gamma_1 = alpha_1 * beta_1;
gamma_2 = alpha_2 * beta_2;
count++;
}
/**
Handle the cases of one of the alphas and betas being 0
Sorted by ease of handling and then frequency
of occurrence
If B is of form
x x 0
0 x 0
0 0 x
*/
if (btFabs(beta_2) <= tol) {
process<0>(B, U, sigma, V);
sort(U, sigma, V,0);
}
/**
If B is of form
x 0 0
0 x x
0 0 x
*/
else if (btFabs(beta_1) <= tol) {
process<1>(B, U, sigma, V);
sort(U, sigma, V,1);
}
/**
If B is of form
x x 0
0 0 x
0 0 x
*/
else if (btFabs(alpha_2) <= tol) {
/**
Reduce B to
x x 0
0 0 0
0 0 x
*/
GivensRotation r1(1, 2);
r1.computeUnconventional(B[1][2], B[2][2]);
r1.rowRotation(B);
r1.columnRotation(U);
process<0>(B, U, sigma, V);
sort(U, sigma, V, 0);
}
/**
If B is of form
x x 0
0 x x
0 0 0
*/
else if (btFabs(alpha_3) <= tol) {
/**
Reduce B to
x x +
0 x 0
0 0 0
*/
GivensRotation r1(1, 2);
r1.compute(B[1][1], B[1][2]);
r1.columnRotation(B);
r1.columnRotation(V);
/**
Reduce B to
x x 0
+ x 0
0 0 0
*/
GivensRotation r2(0, 2);
r2.compute(B[0][0], B[0][2]);
r2.columnRotation(B);
r2.columnRotation(V);
process<0>(B, U, sigma, V);
sort(U, sigma, V, 0);
}
/**
If B is of form
0 x 0
0 x x
0 0 x
*/
else if (btFabs(alpha_1) <= tol) {
/**
Reduce B to
0 0 +
0 x x
0 0 x
*/
GivensRotation r1(0, 1);
r1.computeUnconventional(B[0][1], B[1][1]);
r1.rowRotation(B);
r1.columnRotation(U);
/**
Reduce B to
0 0 0
0 x x
0 + x
*/
GivensRotation r2(0, 2);
r2.computeUnconventional(B[0][2], B[2][2]);
r2.rowRotation(B);
r2.columnRotation(U);
process<1>(B, U, sigma, V);
sort(U, sigma, V, 1);
}
return count;
}
#endif /* btImplicitQRSVD_h */