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

Merge pull request #2775 from xhan0619/splitImpulse

Browse Source 
Split impulse
This commit is contained in:
erwincoumans 2020-04-29 20:00:00 -07:00 committed by GitHub
commit ca50714996
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
28 changed files with 457 additions and 282 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
data/torus_deform.urdf
View File

@ -6,6 +6,7 @@
<inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
<collision_margin value="0.006"/>
<repulsion_stiffness value="50.0"/>
<friction value= "0.5"/>
<neohookean mu= "60" lambda= "200" damping= "0.01" />
<visual filename="torus.vtk"/>

52
examples/DeformableDemo/GraspDeformable.cpp
View File

@ -73,10 +73,10 @@ public:
void resetCamera()
{
float dist = 2;
float dist = 0.3;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -0, 0};
float targetPos[3] = {0, -0.1, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@ -105,12 +105,12 @@ public:
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(2);
motor->setMaxAppliedImpulse(20);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(2);
motor->setMaxAppliedImpulse(20);
}
motor->setMaxAppliedImpulse(1);
}
@ -208,9 +208,9 @@ void GraspDeformable::initPhysics()
bool canSleep = false;
bool selfCollide = true;
int numLinks = 2;
btVector3 linkHalfExtents(.1, .2, .04);
btVector3 baseHalfExtents(.1, 0.02, .2);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, .7f,0.f), linkHalfExtents, baseHalfExtents, false);
btVector3 linkHalfExtents(0.02, 0.018, .003);
btVector3 baseHalfExtents(0.02, 0.002, .002);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), linkHalfExtents, baseHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
@ -258,7 +258,7 @@ void GraspDeformable::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -5.3, 0));
groundTransform.setOrigin(btVector3(0, -5.1, 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.);
@ -309,7 +309,7 @@ void GraspDeformable::initPhysics()
// 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->getCollisionShape()->setMargin(0.01);
psb->setMaxStress(50);
psb->setTotalMass(.1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -334,8 +334,8 @@ void GraspDeformable::initPhysics()
if(1)
{
bool onGround = false;
const btScalar s = .5;
const btScalar h = .1;
const btScalar s = .05;
const btScalar h = -0.02;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
@ -352,37 +352,35 @@ void GraspDeformable::initPhysics()
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.01);
psb->getCollisionShape()->setMargin(0.003);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->setTotalMass(0.005);
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 = 2;
psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
// 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(10,1, true));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
slider.m_minVal = -.2;
slider.m_maxVal = .2;
slider.m_minVal = -.02;
slider.m_maxVal = .02;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -.5;
slider.m_maxVal = .5;
slider.m_minVal = -1;
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
@ -436,8 +434,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 100.f;
float linkMass = 100.f;
float baseMass = 0.1;
float linkMass = 0.1;
int numLinks = 2;
if (baseMass)
@ -465,8 +463,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
//y-axis assumed up
btAlignedObjectArray<btVector3> parentComToCurrentCom;
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 4.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 4.f));//par body's COM to cur body's COM offset
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 2.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*8.f, 0); //cur body's COM to cur body's PIV offset
@ -507,6 +505,7 @@ void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsW
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
@ -537,6 +536,7 @@ void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsW
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);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);

12
examples/DeformableDemo/Pinch.cpp
View File

@ -300,21 +300,21 @@ void Pinch::initPhysics()
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.kDF = .5;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
getDeformableDynamicsWorld()->addSoftBody(psb);
btSoftBodyHelpers::generateBoundaryFaces(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05);
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);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,3, 0.02);
neohookean->setPoissonRatio(0.3);
neohookean->setYoungsModulus(25);
neohookean->setDamping(0.01);
psb->m_cfg.drag = 0.001;
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
// add a grippers

2
examples/DeformableDemo/SplitImpulse.cpp
View File

@ -165,7 +165,7 @@ void SplitImpulse::initPhysics()
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 = 0.1;
psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
getDeformableDynamicsWorld()->addSoftBody(psb);

11
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@ -1163,6 +1163,17 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
}
deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
XMLElement* repulsion_xml = config->FirstChildElement("repulsion_stiffness");
if (repulsion_xml)
{
if (!repulsion_xml->Attribute("value"))
{
logger->reportError("repulsion_stiffness element must have value attribute");
return false;
}
deformable.m_repulsionStiffness = urdfLexicalCast<double>(repulsion_xml->Attribute("value"));
}
XMLElement* spring_xml = config->FirstChildElement("spring");
if (spring_xml)

3
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -225,6 +225,7 @@ struct UrdfDeformable
double m_mass;
double m_collisionMargin;
double m_friction;
double m_repulsionStiffness;
SpringCoeffcients m_springCoefficients;
LameCoefficients m_corotatedCoefficients;
@ -234,7 +235,7 @@ struct UrdfDeformable
std::string m_simFileName;
btHashMap<btHashString, std::string> m_userData;
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("")
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_visualFileName(""), m_simFileName("")
{
}
};

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -404,6 +404,15 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_repulsionStiffness = stiffness;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -648,7 +648,8 @@ extern "C"
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 b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness);
B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness);

5
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -8312,6 +8312,10 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
{
deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff;
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS)
{
deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness;
}
#endif
}
@ -8531,6 +8535,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
psb->setCollisionFlags(0);
psb->setTotalMass(deformable.m_mass);
psb->setSelfCollision(useSelfCollision);
psb->setSpringStiffness(deformable.m_repulsionStiffness);
psb->initializeFaceTree();
}
#endif //SKIP_DEFORMABLE_BODY

24
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -497,17 +497,18 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_UPDATE_MASS = 1<<2,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 1<<3,
LOAD_SOFT_BODY_INITIAL_POSITION = 1<<4,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5,
LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6,
LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7,
LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8,
LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9,
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_USE_SELF_COLLISION = 1<<13,
LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
LOAD_SOFT_BODY_SIM_MESH = 1<<15,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5,
LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6,
LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7,
LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8,
LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9,
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_USE_SELF_COLLISION = 1<<13,
LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
LOAD_SOFT_BODY_SIM_MESH = 1<<15,
LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16,
};
enum EnumSimParamInternalSimFlags
@ -540,6 +541,7 @@ struct LoadSoftBodyArgs
double m_NeoHookeanDamping;
int m_useFaceContact;
char m_simFileName[MAX_FILENAME_LENGTH];
double m_repulsionStiffness;
};
struct b3LoadSoftBodyResultArgs

3
examples/pybullet/examples/deformable_torus.py
View File

@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 50)
bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION)
@ -23,4 +23,3 @@ p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,-10)
sleep(1./240.)

10
examples/pybullet/pybullet.c
View File

@ -2038,7 +2038,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
int physicsClientId = 0;
int flags = 0;
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL};
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL};
int bodyUniqueId = -1;
const char* fileName = "";
@ -2057,7 +2057,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
double frictionCoeff = 0;
int useFaceContact = 0;
int useSelfCollision = 0;
double repulsionStiffness = 0.5;
b3PhysicsClientHandle sm = 0;
@ -2068,7 +2068,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
PyObject* basePosObj = 0;
PyObject* baseOrnObj = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiidddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiidddddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId))
{
return NULL;
}
@ -2134,6 +2134,10 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
{
b3LoadSoftBodySetSelfCollision(command, useSelfCollision);
}
if (repulsionStiffness > 0)
{
b3LoadSoftBodySetRepulsionStiffness(command, repulsionStiffness);
}
b3LoadSoftBodySetFrictionCoefficient(command, frictionCoeff);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);

1
src/BulletSoftBody/CMakeLists.txt
View File

@ -59,6 +59,7 @@ SET(BulletSoftBody_HDRS
btDeformableContactProjection.h
btDeformableMultiBodyDynamicsWorld.h
btDeformableContactConstraint.h
btKrylovSolver.h
poly34.h
btSoftBodySolverVertexBuffer.h

85
src/BulletSoftBody/btConjugateGradient.h
View File

@ -15,24 +15,17 @@
#ifndef BT_CONJUGATE_GRADIENT_H
#define BT_CONJUGATE_GRADIENT_H
#include <iostream>
#include <cmath>
#include <limits>
#include <LinearMath/btAlignedObjectArray.h>
#include <LinearMath/btVector3.h>
#include "LinearMath/btQuickprof.h"
#include "btKrylovSolver.h"
template <class MatrixX>
class btConjugateGradient
class btConjugateGradient : public btKrylovSolver<MatrixX>
{
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base;
TVStack r,p,z,temp;
int max_iterations;
btScalar tolerance_squared;
public:
btConjugateGradient(const int max_it_in)
: max_iterations(max_it_in)
: btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
{
tolerance_squared = 1e-5;
}
virtual ~btConjugateGradient(){}
@ -45,13 +38,13 @@ public:
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp);
r = sub(b, temp);
r = this->sub(b, temp);
A.project(r);
// z = M^(-1) * r
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = dot(z,r);
if (r_dot_z <= tolerance_squared) {
btScalar r_dot_z = this->dot(z,r);
if (r_dot_z <= Base::m_tolerance) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
@ -61,11 +54,11 @@ public:
}
p = z;
btScalar r_dot_z_new = r_dot_z;
for (int k = 1; k <= max_iterations; k++) {
for (int k = 1; k <= Base::m_maxIterations; k++) {
// temp = A*p
A.multiply(p, temp);
A.project(temp);
if (dot(p,temp) < SIMD_EPSILON)
if (this->dot(p,temp) < SIMD_EPSILON)
{
if (verbose)
std::cout << "Encountered negative direction in CG!" << std::endl;
@ -76,16 +69,16 @@ public:
return k;
}
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z_new / dot(p, temp);
btScalar alpha = r_dot_z_new / this->dot(p, temp);
// x += alpha * p;
multAndAddTo(alpha, p, x);
this->multAndAddTo(alpha, p, x);
// r -= alpha * temp;
multAndAddTo(-alpha, temp, r);
this->multAndAddTo(-alpha, temp, r);
// z = M^(-1) * r
A.precondition(r, z);
r_dot_z = r_dot_z_new;
r_dot_z_new = dot(r,z);
if (r_dot_z_new < tolerance_squared) {
r_dot_z_new = this->dot(r,z);
if (r_dot_z_new < Base::m_tolerance) {
if (verbose)
{
std::cout << "ConjugateGradient iterations " << k << std::endl;
@ -94,13 +87,13 @@ public:
}
btScalar beta = r_dot_z_new/r_dot_z;
p = multAndAdd(beta, p, z);
p = this->multAndAdd(beta, p, z);
}
if (verbose)
{
std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl;
}
return max_iterations;
return Base::m_maxIterations;
}
void reinitialize(const TVStack& b)
@ -110,49 +103,5 @@ public:
z.resize(b.size());
temp.resize(b.size());
}
TVStack sub(const TVStack& a, const TVStack& b)
{
// c = a-b
btAssert(a.size() == b.size());
TVStack c;
c.resize(a.size());
for (int i = 0; i < a.size(); ++i)
{
c[i] = a[i] - b[i];
}
return c;
}
btScalar squaredNorm(const TVStack& a)
{
return dot(a,a);
}
btScalar dot(const TVStack& a, const TVStack& b)
{
btScalar ans(0);
for (int i = 0; i < a.size(); ++i)
ans += a[i].dot(b[i]);
return ans;
}
void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
{
// result += s*a
btAssert(a.size() == result.size());
for (int i = 0; i < a.size(); ++i)
result[i] += s * a[i];
}
TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
{
// result = a*s + b
TVStack result;
result.resize(a.size());
for (int i = 0; i < a.size(); ++i)
result[i] = s * a[i] + b[i];
return result;
}
};
#endif /* btConjugateGradient_h */

121
src/BulletSoftBody/btConjugateResidual.h
View File

@ -15,28 +15,22 @@
#ifndef BT_CONJUGATE_RESIDUAL_H
#define BT_CONJUGATE_RESIDUAL_H
#include <iostream>
#include <cmath>
#include <limits>
#include <LinearMath/btAlignedObjectArray.h>
#include <LinearMath/btVector3.h>
#include <LinearMath/btScalar.h>
#include "LinearMath/btQuickprof.h"
#include "btKrylovSolver.h"
template <class MatrixX>
class btConjugateResidual
class btConjugateResidual : public btKrylovSolver<MatrixX>
{
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base;
TVStack r,p,z,temp_p, temp_r, best_x;
// temp_r = A*r
// temp_p = A*p
// z = M^(-1) * temp_p = M^(-1) * A * p
int max_iterations;
btScalar tolerance_squared, best_r;
btScalar best_r;
public:
btConjugateResidual(const int max_it_in)
: max_iterations(max_it_in)
: Base(max_it_in, 1e-8)
{
tolerance_squared = 1e-2;
}
virtual ~btConjugateResidual(){}
@ -49,17 +43,12 @@ public:
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp_r); // borrow temp_r here to store A*x
r = sub(b, temp_r);
r = this->sub(b, temp_r);
// z = M^(-1) * r
A.precondition(r, z); // borrow z to store preconditioned r
r = z;
btScalar residual_norm = norm(r);
if (residual_norm <= tolerance_squared) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << residual_norm << std::endl;
}
btScalar residual_norm = this->norm(r);
if (residual_norm <= Base::m_tolerance) {
return 0;
}
p = r;
@ -68,52 +57,41 @@ public:
A.multiply(p, temp_p);
// temp_r = A*r
temp_r = temp_p;
r_dot_Ar = dot(r, temp_r);
for (int k = 1; k <= max_iterations; k++) {
r_dot_Ar = this->dot(r, temp_r);
for (int k = 1; k <= Base::m_maxIterations; k++) {
// z = M^(-1) * Ap
A.precondition(temp_p, z);
// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
btScalar alpha = r_dot_Ar / dot(temp_p, z);
btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
// x += alpha * p;
multAndAddTo(alpha, p, x);
this->multAndAddTo(alpha, p, x);
// r -= alpha * z;
multAndAddTo(-alpha, z, r);
btScalar norm_r = norm(r);
this->multAndAddTo(-alpha, z, r);
btScalar norm_r = this->norm(r);
if (norm_r < best_r)
{
best_x = x;
best_r = norm_r;
if (norm_r < tolerance_squared) {
if (verbose)
{
std::cout << "ConjugateResidual iterations " << k << std::endl;
}
if (norm_r < Base::m_tolerance) {
return k;
}
else
{
if (verbose)
{
std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl;
}
}
}
// temp_r = A * r;
A.multiply(r, temp_r);
r_dot_Ar_new = dot(r, temp_r);
r_dot_Ar_new = this->dot(r, temp_r);
btScalar beta = r_dot_Ar_new/r_dot_Ar;
r_dot_Ar = r_dot_Ar_new;
// p = beta*p + r;
p = multAndAdd(beta, p, r);
p = this->multAndAdd(beta, p, r);
// temp_p = beta*temp_p + temp_r;
temp_p = multAndAdd(beta, temp_p, temp_r);
temp_p = this->multAndAdd(beta, temp_p, temp_r);
}
if (verbose)
{
std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl;
std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
}
x = best_x;
return max_iterations;
return Base::m_maxIterations;
}
void reinitialize(const TVStack& b)
@ -126,63 +104,6 @@ public:
best_x.resize(b.size());
best_r = SIMD_INFINITY;
}
TVStack sub(const TVStack& a, const TVStack& b)
{
// c = a-b
btAssert(a.size() == b.size());
TVStack c;
c.resize(a.size());
for (int i = 0; i < a.size(); ++i)
{
c[i] = a[i] - b[i];
}
return c;
}
btScalar squaredNorm(const TVStack& a)
{
return dot(a,a);
}
btScalar norm(const TVStack& a)
{
btScalar ret = 0;
for (int i = 0; i < a.size(); ++i)
{
for (int d = 0; d < 3; ++d)
{
ret = btMax(ret, btFabs(a[i][d]));
}
}
return ret;
}
btScalar dot(const TVStack& a, const TVStack& b)
{
btScalar ans(0);
for (int i = 0; i < a.size(); ++i)
ans += a[i].dot(b[i]);
return ans;
}
void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
{
// result += s*a
btAssert(a.size() == result.size());
for (int i = 0; i < a.size(); ++i)
result[i] += s * a[i];
}
TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
{
// result = a*s + b
TVStack result;
result.resize(a.size());
for (int i = 0; i < a.size(); ++i)
result[i] = s * a[i] + b[i];
return result;
}
};
#endif /* btConjugateResidual_h */

25
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@ -168,6 +168,31 @@ public:
}
}
}
void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
{
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
const btSoftBody::Node& node = psb->m_nodes[j];
f[counter] = (node.m_im == 0) ? btVector3(0,0,0) : dv[counter] / node.m_im;
++counter;
}
}
for (int i = 0; i < m_lf.size(); ++i)
{
// add damping matrix
m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
}
counter = 0;
for (; counter < f.size(); ++counter)
{
f[counter] = rhs[counter] - f[counter];
}
}
};
#endif /* btBackwardEulerObjective_h */

10
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -262,11 +262,6 @@ btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** def
return maxSquaredResidual;
}
void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
m_objective->m_projection.splitImpulseSetup(infoGlobal);
}
void btDeformableBodySolver::updateVelocity()
{
int counter = 0;
@ -286,7 +281,7 @@ void btDeformableBodySolver::updateVelocity()
{
m_dv[counter].setZero();
}
psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter;
}
@ -349,7 +344,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
}
else
{
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
}
psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter;
@ -441,6 +436,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
n.m_v *= max_v;
}
n.m_q = n.m_x + n.m_v * dt;
n.m_splitv.setZero();
n.m_penetration = 0;
}

3
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -67,9 +67,6 @@ public:
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// set up the position error in split impulse
void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
// resize/clear data structures
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);

109
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -142,13 +142,16 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS
m_total_tangent_dv.setZero();
// The magnitude of penetration is the depth of penetration.
m_penetration = c.m_cti.m_offset;
// m_penetration = btMin(btScalar(0),c.m_cti.m_offset);
m_total_split_impulse = 0;
m_binding = false;
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
, m_penetration(other.m_penetration)
, m_total_split_impulse(other.m_total_split_impulse)
, m_binding(other.m_binding)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
@ -213,12 +216,28 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
btScalar dn = btDot(vr, cti.m_normal);
if (!infoGlobal.m_splitImpulse)
{
dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
}
// 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_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ;
const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
btVector3 impulse = m_contact->m_c0 * vr;
if (!infoGlobal.m_splitImpulse)
{
impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
}
btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
if (dn > 0)
{
dn = 0;
impulse_normal.setZero();
m_binding = false;
return 0;
}
m_binding = true;
btScalar residualSquare = dn*dn;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
m_total_normal_dv -= impulse_normal * m_contact->m_c2;
@ -228,7 +247,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
{
// separating in the normal direction
m_static = false;
m_total_tangent_dv = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
@ -257,8 +275,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
impulse = impulse_normal + impulse_tangent;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
if (residualSquare < 1e-7)
return residualSquare;
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
@ -288,13 +304,35 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
}
}
}
// va = getVa();
// vb = getVb();
// vr = vb - va;
// btScalar dn1 = btDot(vr, cti.m_normal) / 150;
// m_penetration += dn1;
return residualSquare;
}
btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
btScalar MAX_PENETRATION_CORRECTION = 0.1;
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 vb = getSplitVb();
btScalar p = m_penetration;
btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
if (dn > 0)
{
return 0;
}
if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION)
{
dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse;
}
if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION)
{
dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse;
}
m_total_split_impulse += dn;
btScalar residualSquare = dn*dn;
const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn);
applySplitImpulse(impulse);
return residualSquare;
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)
@ -313,6 +351,10 @@ btVector3 btDeformableNodeRigidContactConstraint::getVb() const
return m_node->m_v;
}
btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const
{
return m_node->m_splitv;
}
btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
@ -326,6 +368,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
contact->m_node->m_v -= dv;
}
void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_splitv -= dv;
}
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
: m_face(contact.m_face)
@ -441,6 +490,40 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
}
}
btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv)* contact->m_bary[2];
return vb;
}
void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_splitv;
btVector3& v1 = face->m_n[1]->m_splitv;
btVector3& v2 = face->m_n[2]->m_splitv;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
{
v0 -= dv * contact->m_weights[0];
}
if (im1 > 0)
{
v1 -= dv * contact->m_weights[1];
}
if (im2 > 0)
{
v2 -= dv * contact->m_weights[2];
}
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)

21
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -148,6 +148,8 @@ public:
btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv;
btScalar m_penetration;
btScalar m_total_split_impulse;
bool m_binding;
const btSoftBody::DeformableRigidContact* m_contact;
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
@ -160,12 +162,19 @@ public:
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const = 0;
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale)
{
m_penetration *= scale;
}
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
virtual void applySplitImpulse(const btVector3& impulse) = 0;
};
//
@ -186,6 +195,9 @@ public:
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const;
// get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
@ -196,6 +208,8 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@ -215,9 +229,12 @@ public:
// get the velocity of the deformable face at the contact point
virtual btVector3 getVb() const;
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const;
// get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
// cast the contact to the desired type
const btSoftBody::DeformableFaceRigidContact* getContact() const
{
@ -225,6 +242,8 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//

63
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -58,23 +58,27 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
return residualSquare;
}
void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
// node constraints
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
}
// face constraints
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
}
}
btScalar residualSquare = 0;
for (int i = 0; i < numDeformableBodies; ++i)
{
for (int j = 0; j < m_softBodies.size(); ++j)
{
btCollisionObject* psb = m_softBodies[j];
if (psb != deformableBodies[i])
{
continue;
}
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
}
return residualSquare;
}
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
@ -143,15 +147,16 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
continue;
}
btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
m_faceRigidConstraints[i].push_back(constraint);
}
m_faceRigidConstraints[i].push_back(constraint);
// btVector3 va = constraint.getVa();
// btVector3 vb = constraint.getVb();
// const btVector3 vr = vb - va;
// const btSoftBody::sCti& cti = contact.m_cti;
// const btScalar dn = btDot(vr, cti.m_normal);
// if (dn < SIMD_EPSILON)
// {
// m_faceRigidConstraints[i].push_back(constraint);
// }
}
}
}
@ -499,6 +504,10 @@ void btDeformableContactProjection::setLagrangeMultiplier()
}
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
if (!m_nodeRigidConstraints[i][j].m_binding)
{
continue;
}
int index = m_nodeRigidConstraints[i][j].m_node->index;
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
LagrangeMultiplier lm;
@ -521,6 +530,10 @@ void btDeformableContactProjection::setLagrangeMultiplier()
}
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
if (!m_faceRigidConstraints[i][j].m_binding)
{
continue;
}
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;

2
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -91,7 +91,7 @@ public:
virtual void reinitialize(bool nodeUpdated);
virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
btScalar solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
virtual void setLagrangeMultiplier();

14
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -21,7 +21,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
{
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
@ -41,7 +41,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
if (iteration >= (maxIterations - 1))
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
m_analyticsData.m_numSolverCalls++;
m_analyticsData.m_numIterationsUsed = iteration+1;
@ -105,14 +106,13 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
}
}
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
int iteration;
if (infoGlobal.m_splitImpulse)
{
{
// m_deformableSolver->splitImpulseSetup(infoGlobal);
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
btScalar leastSquaresResidual = 0.f;
@ -128,12 +128,14 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
}
// solve the position correction between deformable and rigid/multibody
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
if (iteration >= (infoGlobal.m_numIterations - 1))
printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
break;
}

2
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@ -44,7 +44,7 @@ protected:
// write the velocity of the underlying rigid body to the the the solver body
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
public:

5
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -61,7 +61,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
m_internalTime = 0.0;
m_implicit = false;
m_lineSearch = false;
m_useProjection = true;
m_useProjection = false;
m_ccdIterations = 5;
m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
}
@ -315,7 +315,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
node.m_v[c] = -clampDeltaV;
}
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
@ -537,6 +537,7 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
}
else
{
m_deformableBodySolver->m_useProjection = false;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
}

108
src/BulletSoftBody/btKrylovSolver.h Normal file
View File

@ -0,0 +1,108 @@
/*
Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
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 BT_KRYLOV_SOLVER_H
#define BT_KRYLOV_SOLVER_H
#include <iostream>
#include <cmath>
#include <limits>
#include <LinearMath/btAlignedObjectArray.h>
#include <LinearMath/btVector3.h>
#include <LinearMath/btScalar.h>
#include "LinearMath/btQuickprof.h"
template <class MatrixX>
class btKrylovSolver
{
typedef btAlignedObjectArray<btVector3> TVStack;
public:
int m_maxIterations;
btScalar m_tolerance;
btKrylovSolver(int maxIterations, btScalar tolerance)
: m_maxIterations(maxIterations)
, m_tolerance(tolerance)
{
}
virtual ~btKrylovSolver(){}
virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0;
virtual void reinitialize(const TVStack& b) = 0;
virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b)
{
// c = a-b
btAssert(a.size() == b.size());
TVStack c;
c.resize(a.size());
for (int i = 0; i < a.size(); ++i)
{
c[i] = a[i] - b[i];
}
return c;
}
virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a)
{
return dot(a,a);
}
virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a)
{
btScalar ret = 0;
for (int i = 0; i < a.size(); ++i)
{
for (int d = 0; d < 3; ++d)
{
ret = btMax(ret, btFabs(a[i][d]));
}
}
return ret;
}
virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b)
{
btScalar ans(0);
for (int i = 0; i < a.size(); ++i)
ans += a[i].dot(b[i]);
return ans;
}
virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
{
// result += s*a
btAssert(a.size() == result.size());
for (int i = 0; i < a.size(); ++i)
result[i] += s * a[i];
}
virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
{
// result = a*s + b
TVStack result;
result.resize(a.size());
for (int i = 0; i < a.size(); ++i)
result[i] = s * a[i] + b[i];
return result;
}
virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance)
{
m_tolerance = tolerance;
}
};
#endif /* BT_KRYLOV_SOLVER_H */

30
src/BulletSoftBody/btSoftBody.cpp
View File

@ -2911,6 +2911,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
//
// regular face contact
if (0)
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
@ -2932,9 +2933,34 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
cti.m_normal = nrm;
cti.m_offset = dst + (curr - contact_point).dot(nrm);
}
return (dst < 0);
if(1)
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_q);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance-csh->getMargin();
dst -= margin;
if (dst >= 0)
return false;
wtr = colObjWrap->getWorldTransform();
btTriangleShape triangle2(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
triangle_transform.setOrigin(f.m_n[0]->m_x);
btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
dst = results.distance-csh->getMargin()-margin;
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = results.normal;
cti.m_offset = dst;
return true;
}
return true;
}
//
void btSoftBody::updateNormals()
{

5
src/BulletSoftBody/btSoftBody.h
View File

@ -271,7 +271,8 @@ public:
btDbvtNode* m_leaf; // Leaf data
btScalar m_penetration; // depth of penetration
int m_battach : 1; // Attached
int index;
int index;
btVector3 m_splitv; // velocity associated with split impulse
};
/* Link */
ATTRIBUTE_ALIGNED16(struct)
@ -296,7 +297,7 @@ public:
btDbvtNode* m_leaf; // Leaf data
btVector4 m_pcontact; // barycentric weights of the persistent contact
btVector3 m_n0, m_n1, m_vn;
int m_index;
int m_index;
};
/* Tetra */
struct Tetra : Feature