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 remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans 2020-06-18 20:53:21 -07:00
commit 696a4f00cc
37 changed files with 7632 additions and 7677 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
examples/SharedMemory/PhysicsClientTCP.cpp
View File

@ -64,9 +64,10 @@ struct TcpNetworkedInternalData
{ {
m_tcpSocket.SetSendTimeout(m_timeOutInSeconds, 0); m_tcpSocket.SetSendTimeout(m_timeOutInSeconds, 0);
m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds, 0); m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds, 0);
int key = SHARED_MEMORY_MAGIC_NUMBER;
m_tcpSocket.Send((uint8*)&key, 4);
} }
int key = SHARED_MEMORY_MAGIC_NUMBER;
m_tcpSocket.Send((uint8*)&key, 4);
return m_isConnected; return m_isConnected;
} }

334
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -93,11 +93,10 @@
#include "../TinyAudio/b3SoundEngine.h" #include "../TinyAudio/b3SoundEngine.h"
#endif #endif
#ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#define SKIP_DEFORMABLE_BODY 1 #define SKIP_DEFORMABLE_BODY 1
#endif #endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftBodySolvers.h" #include "BulletSoftBody/btSoftBodySolvers.h"
@ -107,18 +106,16 @@
#include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#include "../SoftDemo/BunnyMesh.h" #include "../SoftDemo/BunnyMesh.h"
#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#endif//SKIP_DEFORMABLE_BODY #endif //SKIP_DEFORMABLE_BODY
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
int gInternalSimFlags = 0; int gInternalSimFlags = 0;
bool gResetSimulation = 0; bool gResetSimulation = 0;
int gVRTrackingObjectUniqueId = -1; int gVRTrackingObjectUniqueId = -1;
@ -1617,12 +1614,9 @@ struct PhysicsServerCommandProcessorInternalData
btScalar getDeltaTimeSubStep() const btScalar getDeltaTimeSubStep() const
{ {
btScalar deltaTimeSubStep = m_numSimulationSubSteps > 0 ? btScalar deltaTimeSubStep = m_numSimulationSubSteps > 0 ? m_physicsDeltaTime / m_numSimulationSubSteps : m_physicsDeltaTime;
m_physicsDeltaTime / m_numSimulationSubSteps :
m_physicsDeltaTime;
return deltaTimeSubStep; return deltaTimeSubStep;
} }
btScalar m_simulationTimestamp; btScalar m_simulationTimestamp;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks; btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
@ -1653,15 +1647,14 @@ struct PhysicsServerCommandProcessorInternalData
btDefaultCollisionConfiguration* m_collisionConfiguration; btDefaultCollisionConfiguration* m_collisionConfiguration;
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
btSoftBody* m_pickedSoftBody; btSoftBody* m_pickedSoftBody;
btDeformableMousePickingForce* m_mouseForce; btDeformableMousePickingForce* m_mouseForce;
btScalar m_maxPickingForce; btScalar m_maxPickingForce;
btDeformableBodySolver* m_deformablebodySolver; btDeformableBodySolver* m_deformablebodySolver;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#endif #endif
btMultiBodyDynamicsWorld* m_dynamicsWorld; btMultiBodyDynamicsWorld* m_dynamicsWorld;
int m_constraintSolverType; int m_constraintSolverType;
@ -1687,7 +1680,7 @@ struct PhysicsServerCommandProcessorInternalData
//data for picking objects //data for picking objects
class btRigidBody* m_pickedBody; class btRigidBody* m_pickedBody;
int m_savedActivationState; int m_savedActivationState;
class btTypedConstraint* m_pickedConstraint; class btTypedConstraint* m_pickedConstraint;
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
@ -2631,9 +2624,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld() btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld()
{ {
btDeformableMultiBodyDynamicsWorld* world = 0; btDeformableMultiBodyDynamicsWorld* world = 0;
if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD) if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType() == BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD)
{ {
world = (btDeformableMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world = (btDeformableMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
} }
return world; return world;
} }
@ -2641,9 +2634,9 @@ btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformable
btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld() btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld()
{ {
btSoftMultiBodyDynamicsWorld* world = 0; btSoftMultiBodyDynamicsWorld* world = 0;
if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD) if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType() == BT_SOFT_MULTIBODY_DYNAMICS_WORLD)
{ {
world = (btSoftMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world = (btSoftMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
} }
return world; return world;
} }
@ -2669,43 +2662,44 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback); m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
//int maxProxies = 32768; //int maxProxies = 32768;
if (flags&RESET_USE_SIMPLE_BROADPHASE) if (flags & RESET_USE_SIMPLE_BROADPHASE)
{ {
m_data->m_broadphase = new btSimpleBroadphase(65536, m_data->m_pairCache); m_data->m_broadphase = new btSimpleBroadphase(65536, m_data->m_pairCache);
} else }
else
{ {
btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache); btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
bv->setVelocityPrediction(0); bv->setVelocityPrediction(0);
m_data->m_broadphase = bv; m_data->m_broadphase = bv;
} }
if (flags & RESET_USE_DEFORMABLE_WORLD) if (flags & RESET_USE_DEFORMABLE_WORLD)
{ {
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
m_data->m_deformablebodySolver = new btDeformableBodySolver(); m_data->m_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver; m_data->m_solver = solver;
solver->setDeformableSolver(m_data->m_deformablebodySolver); solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
#endif #endif
} }
if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) if ((0 == m_data->m_dynamicsWorld) && (0 == (flags & RESET_USE_DISCRETE_DYNAMICS_WORLD)))
{ {
m_data->m_solver = new btMultiBodyConstraintSolver; m_data->m_solver = new btMultiBodyConstraintSolver;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#else #else
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif #endif
} }
if (0==m_data->m_dynamicsWorld) if (0 == m_data->m_dynamicsWorld)
{ {
m_data->m_solver = new btMultiBodyConstraintSolver; m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
} }
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024); m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024);
@ -2863,43 +2857,42 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
delete mb; delete mb;
} }
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
for (int j = 0; j < m_data->m_lf.size(); j++) for (int j = 0; j < m_data->m_lf.size(); j++)
{ {
btDeformableLagrangianForce* force = m_data->m_lf[j]; btDeformableLagrangianForce* force = m_data->m_lf[j];
delete force; delete force;
} }
m_data->m_lf.clear(); m_data->m_lf.clear();
#endif #endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{ {
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld) if (softWorld)
{ {
for (i =softWorld->getSoftBodyArray().size() - 1; i >= 0; i--) for (i = softWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{ {
btSoftBody* sb =softWorld->getSoftBodyArray()[i]; btSoftBody* sb = softWorld->getSoftBodyArray()[i];
softWorld->removeSoftBody(sb); softWorld->removeSoftBody(sb);
delete sb; delete sb;
} }
} }
} }
#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
{ {
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld) if (deformWorld)
{ {
for (i =deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) for (i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{ {
btSoftBody* sb =deformWorld->getSoftBodyArray()[i]; btSoftBody* sb = deformWorld->getSoftBodyArray()[i];
deformWorld->removeSoftBody(sb); deformWorld->removeSoftBody(sb);
delete sb; delete sb;
} }
} }
} }
#endif #endif
} }
for (int i = 0; i < constraints.size(); i++) for (int i = 0; i < constraints.size(); i++)
@ -3037,7 +3030,8 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
} }
} }
int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType) { int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType)
{
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (!body) if (!body)
{ {
@ -3068,13 +3062,16 @@ int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex,
return userDataHandle; return userDataHandle;
} }
void PhysicsServerCommandProcessor::addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex, int visualShapeIndex) { void PhysicsServerCommandProcessor::addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex, int visualShapeIndex)
for (int i = 0; i < user_data_entries.size(); ++i) { {
for (int i = 0; i < user_data_entries.size(); ++i)
{
const std::string key = user_data_entries.getKeyAtIndex(i).m_string1; const std::string key = user_data_entries.getKeyAtIndex(i).m_string1;
const std::string* value = user_data_entries.getAtIndex(i); const std::string* value = user_data_entries.getAtIndex(i);
if (value) { if (value)
{
addUserData(bodyUniqueId, linkIndex, visualShapeIndex, key.c_str(), value->c_str(), addUserData(bodyUniqueId, linkIndex, visualShapeIndex, key.c_str(), value->c_str(),
value->size()+1, USER_DATA_VALUE_TYPE_STRING); value->size() + 1, USER_DATA_VALUE_TYPE_STRING);
} }
} }
} }
@ -3357,23 +3354,29 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
// create a mapping from link name to link index in order to apply the user // create a mapping from link name to link index in order to apply the user
// data to the correct link in the MultiBody. // data to the correct link in the MultiBody.
btHashMap<btHashString, int> linkNameToIndexMap; btHashMap<btHashString, int> linkNameToIndexMap;
if (bodyHandle->m_multiBody) { if (bodyHandle->m_multiBody)
{
btMultiBody* mb = bodyHandle->m_multiBody; btMultiBody* mb = bodyHandle->m_multiBody;
linkNameToIndexMap.insert(mb->getBaseName(), -1); linkNameToIndexMap.insert(mb->getBaseName(), -1);
for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex) { for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
{
linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex); linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
} }
} }
const UrdfModel* urdfModel = u2b.getUrdfModel(); const UrdfModel* urdfModel = u2b.getUrdfModel();
if (urdfModel) { if (urdfModel)
{
addUserData(urdfModel->m_userData, bodyUniqueId); addUserData(urdfModel->m_userData, bodyUniqueId);
for (int i = 0; i < urdfModel->m_links.size(); ++i) { for (int i = 0; i < urdfModel->m_links.size(); ++i)
{
const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
if (linkIndex) { if (linkIndex)
{
addUserData(link->m_userData, bodyUniqueId, *linkIndex); addUserData(link->m_userData, bodyUniqueId, *linkIndex);
for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) { for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
{
addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
} }
} }
@ -3522,7 +3525,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (loadOk) if (loadOk)
{ {
btTransform rootTrans; btTransform rootTrans;
rootTrans.setOrigin(pos); rootTrans.setOrigin(pos);
rootTrans.setRotation(orn); rootTrans.setRotation(orn);
@ -5095,19 +5097,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{ {
btVector3 pt; btVector3 pt;
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0], pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]);
convexHull->addPoint(pt * meshScale, false); convexHull->addPoint(pt * meshScale, false);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
convexHull->addPoint(pt * meshScale, false); convexHull->addPoint(pt * meshScale, false);
pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
convexHull->addPoint(pt * meshScale, false); convexHull->addPoint(pt * meshScale, false);
} }
@ -5303,7 +5305,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
if (bodyHandle->m_multiBody) if (bodyHandle->m_multiBody)
{ {
//collision shape //collision shape
if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1) if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1)
{ {
@ -5325,22 +5326,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
btTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
int collisionShapeIndex = -1; int collisionShapeIndex = -1;
if (clientCmd.m_updateFlags& B3_MESH_DATA_COLLISIONSHAPEINDEX) if (clientCmd.m_updateFlags & B3_MESH_DATA_COLLISIONSHAPEINDEX)
{ {
collisionShapeIndex = clientCmd.m_requestMeshDataArgs.m_collisionShapeIndex; collisionShapeIndex = clientCmd.m_requestMeshDataArgs.m_collisionShapeIndex;
} }
gatherVertices(tr, colShape, vertices, collisionShapeIndex); gatherVertices(tr, colShape, vertices, collisionShapeIndex);
int numVertices = vertices.size(); int numVertices = vertices.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
if (verticesCopied > 0) if (verticesCopied > 0)
{ {
memcpy(verticesOut, &vertices[0], sizeof(btVector3) * verticesCopied); memcpy(verticesOut, &vertices[0], sizeof(btVector3) * verticesCopied);
} }
sizeInBytes = verticesCopied * sizeof(btVector3); sizeInBytes = verticesCopied * sizeof(btVector3);
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
@ -5353,13 +5354,13 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
if (bodyHandle->m_softBody) if (bodyHandle->m_softBody)
{ {
btSoftBody* psb = bodyHandle->m_softBody; btSoftBody* psb = bodyHandle->m_softBody;
bool separateRenderMesh = (psb->m_renderNodes.size() != 0); bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
for (int i = 0; i < verticesCopied; ++i) for (int i = 0; i < verticesCopied; ++i)
{ {
if (separateRenderMesh) if (separateRenderMesh)
@ -5818,7 +5819,6 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
} }
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
{ {
m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
@ -5974,14 +5974,11 @@ struct CastSyncInfo
}; };
#endif // __cplusplus >= 201103L #endif // __cplusplus >= 201103L
struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{ {
FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask) FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask)
: btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
m_collisionFilterMask(collisionFilterMask) m_collisionFilterMask(collisionFilterMask)
{ {
} }
@ -5999,12 +5996,12 @@ struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayRes
} }
}; };
struct FilteredAllHitsRayResultCallback: public btCollisionWorld::AllHitsRayResultCallback struct FilteredAllHitsRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback
{ {
FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask, btScalar fractionEpsilon) FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask, btScalar fractionEpsilon)
: btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld), : btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld),
m_collisionFilterMask(collisionFilterMask), m_collisionFilterMask(collisionFilterMask),
m_fractionEpsilon(fractionEpsilon) m_fractionEpsilon(fractionEpsilon)
{ {
} }
@ -6027,7 +6024,7 @@ struct FilteredAllHitsRayResultCallback: public btCollisionWorld::AllHitsRayResu
{ {
if (m_collisionObjects[i] == rayResult.m_collisionObject) if (m_collisionObjects[i] == rayResult.m_collisionObject)
{ {
btScalar diffFraction = m_hitFractions[i]-rayResult.m_hitFraction; btScalar diffFraction = m_hitFractions[i] - rayResult.m_hitFraction;
if (btEqual(diffFraction, m_fractionEpsilon)) if (btEqual(diffFraction, m_fractionEpsilon))
{ {
isDuplicate = true; isDuplicate = true;
@ -6055,9 +6052,7 @@ struct BatchRayCaster
btScalar m_fractionEpsilon; btScalar m_fractionEpsilon;
BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask, btScalar fractionEpsilon) BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask, btScalar fractionEpsilon)
: m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber), : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber), m_collisionFilterMask(collisionFilterMask), m_fractionEpsilon(fractionEpsilon)
m_collisionFilterMask(collisionFilterMask),
m_fractionEpsilon(fractionEpsilon)
{ {
m_syncInfo = new CastSyncInfo; m_syncInfo = new CastSyncInfo;
} }
@ -6162,7 +6157,7 @@ struct BatchRayCaster
{ {
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
} }
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (body) if (body)
{ {
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
@ -6399,7 +6394,7 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
int usz = m_data->m_userConstraints.size(); int usz = m_data->m_userConstraints.size();
int* constraintUid = bodyUids + actualNumBodies; int* constraintUid = bodyUids + actualNumBodies;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
for (int i = 0; i < usz; i++) for (int i = 0; i < usz; i++)
{ {
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1(); int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
@ -6424,14 +6419,16 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
} }
else else
{ {
for (int i=0; i<clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies; ++i) { for (int i = 0; i < clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies; ++i)
{
const int bodyUniqueId = clientCmd.m_syncUserDataRequestArgs.m_requestedBodyIds[i]; const int bodyUniqueId = clientCmd.m_syncUserDataRequestArgs.m_requestedBodyIds[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (!body) if (!body)
{ {
return hasStatus; return hasStatus;
} }
for (int j=0; j < body->m_userDataHandles.size(); ++j) { for (int j = 0; j < body->m_userDataHandles.size(); ++j)
{
userDataHandles.push_back(body->m_userDataHandles[j]); userDataHandles.push_back(body->m_userDataHandles[j]);
} }
} }
@ -6446,7 +6443,7 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size(); serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
serverStatusOut.m_numDataStreamBytes = sizeInBytes; serverStatusOut.m_numDataStreamBytes = sizeInBytes;
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
return hasStatus; return hasStatus;
} }
@ -6496,7 +6493,8 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
addUserDataArgs.m_visualShapeIndex, addUserDataArgs.m_key, addUserDataArgs.m_visualShapeIndex, addUserDataArgs.m_key,
bufferServerToClient, addUserDataArgs.m_valueLength, bufferServerToClient, addUserDataArgs.m_valueLength,
addUserDataArgs.m_valueType); addUserDataArgs.m_valueType);
if (userDataHandle < 0) { if (userDataHandle < 0)
{
return hasStatus; return hasStatus;
} }
@ -7345,7 +7343,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
} }
} }
} //fi } //fi
//break; //break;
} }
} }
} //if (body && body->m_rigidBody) } //if (body && body->m_rigidBody)
@ -7659,7 +7657,6 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage); serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
serverCmd.m_sendActualStateArgs.m_stateDetails = 0; serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
body->m_rootLocalInertialFrame.getOrigin()[0]; body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
@ -7676,7 +7673,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3]; body->m_rootLocalInertialFrame.getRotation()[3];
btVector3 center_of_mass(sb->getCenterOfMass()); btVector3 center_of_mass(sb->getCenterOfMass());
btTransform tr = sb->getRigidTransform(); btTransform tr = sb->getRigidTransform();
//base position in world space, cartesian //base position in world space, cartesian
stateDetails->m_actualStateQ[0] = center_of_mass[0]; stateDetails->m_actualStateQ[0] = center_of_mass[0];
@ -7830,7 +7827,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
{ {
pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j]; pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j]; pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j]; pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j];
} }
else else
{ {
@ -8088,9 +8085,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
cb.m_bodyUniqueIdA = bodyUniqueIdA; cb.m_bodyUniqueIdA = bodyUniqueIdA;
cb.m_bodyUniqueIdB = bodyUniqueIdB; cb.m_bodyUniqueIdB = bodyUniqueIdB;
cb.m_deltaTime = m_data->m_numSimulationSubSteps>0 ? cb.m_deltaTime = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
m_data->m_physicsDeltaTime/ m_data->m_numSimulationSubSteps :
m_data->m_physicsDeltaTime;
for (int i = 0; i < setA.size(); i++) for (int i = 0; i < setA.size(); i++)
{ {
@ -8441,7 +8436,6 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
#endif #endif
} }
bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision)
{ {
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@ -8761,7 +8755,6 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
return true; return true;
} }
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{ {
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
@ -8818,11 +8811,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
} }
#endif #endif
return hasStatus; return hasStatus;
} }
bool PhysicsServerCommandProcessor::processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) bool PhysicsServerCommandProcessor::processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
@ -9576,8 +9567,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
{ {
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY; serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY;
btMultiBody* mb = body->m_multiBody; btMultiBody* mb = body->m_multiBody;
if (linkIndex == -1) if (linkIndex == -1)
{ {
@ -9695,7 +9686,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
{ {
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY; serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY;
btRigidBody* rb = body->m_rigidBody; btRigidBody* rb = body->m_rigidBody;
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction(); serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction();
@ -9707,7 +9698,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_collisionMargin = rb->getCollisionShape() ? rb->getCollisionShape()->getMargin() : 0; serverCmd.m_dynamicsInfo.m_collisionMargin = rb->getCollisionShape() ? rb->getCollisionShape()->getMargin() : 0;
} }
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){ else if (body && body->m_softBody)
{
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY; serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;
@ -9842,9 +9834,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
gforce->m_gravity = grav; gforce->m_gravity = grav;
} }
} }
} }
#endif #endif
if (m_data->m_verboseOutput) if (m_data->m_verboseOutput)
@ -9856,8 +9846,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{ {
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
} }
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_NONCONTACT_INNER_ITERATIONS) if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_NONCONTACT_INNER_ITERATIONS)
{ {
m_data->m_dynamicsWorld->getSolverInfo().m_numNonContactInnerIterations = clientCmd.m_physSimParamArgs.m_numNonContactInnerIterations; m_data->m_dynamicsWorld->getSolverInfo().m_numNonContactInnerIterations = clientCmd.m_physSimParamArgs.m_numNonContactInnerIterations;
} }
@ -9922,7 +9912,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{ {
} }
}; };
if (newSolver) if (newSolver)
{ {
delete oldSolver; delete oldSolver;
@ -9987,23 +9977,23 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{ {
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
{ {
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld) if (deformWorld)
{ {
deformWorld ->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); deformWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
deformWorld ->getWorldInfo().m_sparsesdf.Reset(); deformWorld->getWorldInfo().m_sparsesdf.Reset();
} }
} }
#endif #endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{ {
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld) if (softWorld)
{ {
softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
softWorld->getWorldInfo().m_sparsesdf.Reset(); softWorld->getWorldInfo().m_sparsesdf.Reset();
} }
} }
#endif #endif
} }
@ -10041,7 +10031,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{ {
bool hasStatus = true; bool hasStatus = true;
BT_PROFILE("CMD_INIT_POSE"); BT_PROFILE("CMD_INIT_POSE");
if (m_data->m_verboseOutput) if (m_data->m_verboseOutput)
@ -10089,7 +10079,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
if (clientCmd.m_updateFlags & INIT_POSE_HAS_SCALING) if (clientCmd.m_updateFlags & INIT_POSE_HAS_SCALING)
{ {
btVector3 scaling(clientCmd.m_initPoseArgs.m_scaling[0], clientCmd.m_initPoseArgs.m_scaling[1], clientCmd.m_initPoseArgs.m_scaling[2]); btVector3 scaling(clientCmd.m_initPoseArgs.m_scaling[0], clientCmd.m_initPoseArgs.m_scaling[1], clientCmd.m_initPoseArgs.m_scaling[2]);
mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling); mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling);
//refresh broadphase //refresh broadphase
m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs( m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(
@ -10272,7 +10262,7 @@ bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct S
bool hasStatus = true; bool hasStatus = true;
BT_PROFILE("CMD_RESET_SIMULATION"); BT_PROFILE("CMD_RESET_SIMULATION");
m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0); m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0);
resetSimulation(clientCmd.m_updateFlags); resetSimulation(clientCmd.m_updateFlags);
m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1); m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1);
@ -11018,6 +11008,7 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
} }
} }
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (body && body->m_softBody) if (body && body->m_softBody)
{ {
btSoftBody* sb = body->m_softBody; btSoftBody* sb = body->m_softBody;
@ -11025,8 +11016,8 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
{ {
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 positionLocal( btVector3 positionLocal(
clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
@ -11040,6 +11031,8 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
} }
} }
} }
#endif
} }
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
@ -11174,7 +11167,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
{ {
deformWorld->removeSoftBody(psb); deformWorld->removeSoftBody(psb);
} }
int graphicsInstance = psb->getUserIndex2(); int graphicsInstance = psb->getUserIndex2();
m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
delete psb; delete psb;
@ -11268,35 +11261,36 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
if (sbodyHandle->m_softBody) if (sbodyHandle->m_softBody)
{ {
int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex; int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex;
if (nodeIndex>=0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size()) if (nodeIndex >= 0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size())
{ {
int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex; int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex;
if (bodyUniqueId<=0) if (bodyUniqueId <= 0)
{ {
//fixed anchor (mass = 0) //fixed anchor (mass = 0)
InteralUserConstraintData userConstraintData; InteralUserConstraintData userConstraintData;
userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex; userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex;
userConstraintData.m_sbNodeIndex = nodeIndex; userConstraintData.m_sbNodeIndex = nodeIndex;
userConstraintData.m_sbNodeMass = sbodyHandle->m_softBody->getMass(nodeIndex); userConstraintData.m_sbNodeMass = sbodyHandle->m_softBody->getMass(nodeIndex);
sbodyHandle->m_softBody->setMass(nodeIndex,0.0); sbodyHandle->m_softBody->setMass(nodeIndex, 0.0);
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
m_data->m_userConstraints.insert(uid, userConstraintData); m_data->m_userConstraints.insert(uid, userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} else }
else
{ {
InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (mbodyHandle && mbodyHandle->m_multiBody) if (mbodyHandle && mbodyHandle->m_multiBody)
{ {
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld) if (deformWorld)
{ {
int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex; int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex;
if (linkIndex<0) if (linkIndex < 0)
{ {
sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider()); sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider());
} else }
else
{ {
if (linkIndex < mbodyHandle->m_multiBody->getNumLinks()) if (linkIndex < mbodyHandle->m_multiBody->getNumLinks())
{ {
@ -11322,11 +11316,11 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
{ {
bool disableCollisionBetweenLinkedBodies = true; bool disableCollisionBetweenLinkedBodies = true;
btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0], btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]); clientCmd.m_userConstraintArguments.m_childFrame[2]);
sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies); sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies);
} }
#endif #endif
} }
int uid = m_data->m_userConstraintUIDGenerator++; int uid = m_data->m_userConstraintUIDGenerator++;
@ -11336,12 +11330,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
userConstraintData.m_sbNodeIndex = nodeIndex; userConstraintData.m_sbNodeIndex = nodeIndex;
m_data->m_userConstraints.insert(uid, userConstraintData); m_data->m_userConstraints.insert(uid, userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
} }
} }
} }
#endif #endif
@ -11690,9 +11680,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{ {
btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps :
m_data->m_physicsDeltaTime;
serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
@ -11797,6 +11785,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
delete userConstraintPtr->m_rbConstraint; delete userConstraintPtr->m_rbConstraint;
m_data->m_userConstraints.remove(userConstraintUidRemove); m_data->m_userConstraints.remove(userConstraintUidRemove);
} }
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (userConstraintPtr->m_sbHandle >= 0) if (userConstraintPtr->m_sbHandle >= 0)
{ {
InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
@ -11815,6 +11805,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
} }
} }
} }
#endif
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
} }
@ -13753,49 +13744,47 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{ {
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld) if (deformWorld)
{
for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
{ {
btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i]; for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{ {
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),deformWorld->getDrawFlags()); if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), deformWorld->getDrawFlags());
}
} }
} }
} }
}
{ {
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld) if (softWorld)
{
for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++)
{ {
btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i]; for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++)
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{ {
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i];
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),softWorld->getDrawFlags()); if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), softWorld->getDrawFlags());
}
} }
} }
} }
}
#endif #endif
} }
} }
} }
struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
{ {
int m_faceId; int m_faceId;
MyResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) MyResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
: btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
m_faceId(-1) m_faceId(-1)
{ {
} }
@ -13833,7 +13822,6 @@ struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
} }
}; };
bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{ {
if (m_data->m_dynamicsWorld == 0) if (m_data->m_dynamicsWorld == 0)
@ -13994,7 +13982,6 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_pickingMultiBodyPoint2Point = 0; m_data->m_pickingMultiBodyPoint2Point = 0;
} }
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
//deformable/soft body? //deformable/soft body?
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
@ -14006,7 +13993,6 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_pickedSoftBody = 0; m_data->m_pickedSoftBody = 0;
} }
#endif #endif
} }
void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName) void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName)
@ -14273,7 +14259,7 @@ void PhysicsServerCommandProcessor::resetSimulation(int flags)
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld) if (deformWorld)
{ {
deformWorld ->getWorldInfo().m_sparsesdf.Reset(); deformWorld->getWorldInfo().m_sparsesdf.Reset();
} }
} }
{ {

4
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@ -19,7 +19,7 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface
{ {
struct PhysicsServerCommandProcessorInternalData* m_data; struct PhysicsServerCommandProcessorInternalData* m_data;
void resetSimulation(int flags=0); void resetSimulation(int flags = 0);
void createThreadPool(); void createThreadPool();
class btDeformableMultiBodyDynamicsWorld* getDeformableWorld(); class btDeformableMultiBodyDynamicsWorld* getDeformableWorld();
@ -120,7 +120,7 @@ public:
void createJointMotors(class btMultiBody* body); void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld(int flags=0); virtual void createEmptyDynamicsWorld(int flags = 0);
virtual void deleteDynamicsWorld(); virtual void deleteDynamicsWorld();
virtual bool connect() virtual bool connect()

9
src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h
View File

@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
btDeformableMultiBodyConstraintSolver* m_deformableSolver; btDeformableMultiBodyConstraintSolver* m_deformableSolver;
DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver, DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher) btDispatcher* dispatcher)
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
{ {
} }
virtual void processConstraints(int islandId = -1)
virtual void processConstraints(int islandId=-1)
{ {
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0; btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
@ -30,7 +29,7 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1))
{ {
m_deformableSolver->m_analyticsData.m_islandId = islandId; m_deformableSolver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData); m_islandAnalyticsData.push_back(m_solver->m_analyticsData);

144
src/BulletSoftBody/btCGProjection.h
View File

@ -22,85 +22,83 @@
struct DeformableContactConstraint struct DeformableContactConstraint
{ {
const btSoftBody::Node* m_node; const btSoftBody::Node* m_node;
btAlignedObjectArray<const btSoftBody::RContact*> m_contact; btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
btAlignedObjectArray<btVector3> m_total_normal_dv; btAlignedObjectArray<btVector3> m_total_normal_dv;
btAlignedObjectArray<btVector3> m_total_tangent_dv; btAlignedObjectArray<btVector3> m_total_tangent_dv;
btAlignedObjectArray<bool> m_static; btAlignedObjectArray<bool> m_static;
btAlignedObjectArray<bool> m_can_be_dynamic; btAlignedObjectArray<bool> m_can_be_dynamic;
DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node)
{
append(rcontact);
}
DeformableContactConstraint(): m_node(NULL)
{
m_contact.push_back(NULL);
}
void append(const btSoftBody::RContact& rcontact)
{
m_contact.push_back(&rcontact);
m_total_normal_dv.push_back(btVector3(0,0,0));
m_total_tangent_dv.push_back(btVector3(0,0,0));
m_static.push_back(false);
m_can_be_dynamic.push_back(true);
}
void replace(const btSoftBody::RContact& rcontact) DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node)
{ {
m_contact.clear(); append(rcontact);
m_total_normal_dv.clear(); }
m_total_tangent_dv.clear();
m_static.clear(); DeformableContactConstraint() : m_node(NULL)
m_can_be_dynamic.clear(); {
append(rcontact); m_contact.push_back(NULL);
} }
~DeformableContactConstraint() void append(const btSoftBody::RContact& rcontact)
{ {
} m_contact.push_back(&rcontact);
m_total_normal_dv.push_back(btVector3(0, 0, 0));
m_total_tangent_dv.push_back(btVector3(0, 0, 0));
m_static.push_back(false);
m_can_be_dynamic.push_back(true);
}
void replace(const btSoftBody::RContact& rcontact)
{
m_contact.clear();
m_total_normal_dv.clear();
m_total_tangent_dv.clear();
m_static.clear();
m_can_be_dynamic.clear();
append(rcontact);
}
~DeformableContactConstraint()
{
}
}; };
class btCGProjection class btCGProjection
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies; btAlignedObjectArray<btSoftBody*>& m_softBodies;
const btScalar& m_dt; const btScalar& m_dt;
// map from node indices to node pointers // map from node indices to node pointers
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt)
: m_softBodies(softBodies) : m_softBodies(softBodies), m_dt(dt)
, m_dt(dt) {
{ }
}
virtual ~btCGProjection()
virtual ~btCGProjection() {
{ }
}
// apply the constraints
// apply the constraints virtual void project(TVStack& x) = 0;
virtual void project(TVStack& x) = 0;
virtual void setConstraints() = 0;
virtual void setConstraints() = 0;
// update the constraints
// update the constraints virtual btScalar update() = 0;
virtual btScalar update() = 0;
virtual void reinitialize(bool nodeUpdated)
virtual void reinitialize(bool nodeUpdated) {
{ }
}
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) {
{ m_nodes = nodes;
m_nodes = nodes; }
}
}; };
#endif /* btCGProjection_h */ #endif /* btCGProjection_h */

170
src/BulletSoftBody/btConjugateGradient.h
View File

@ -19,89 +19,93 @@
template <class MatrixX> template <class MatrixX>
class btConjugateGradient : public btKrylovSolver<MatrixX> class btConjugateGradient : public btKrylovSolver<MatrixX>
{ {
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base; typedef btKrylovSolver<MatrixX> Base;
TVStack r,p,z,temp; TVStack r, p, z, temp;
public:
btConjugateGradient(const int max_it_in)
: btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
{
}
virtual ~btConjugateGradient(){}
// return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
{
BT_PROFILE("CGSolve");
btAssert(x.size() == b.size());
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp);
r = this->sub(b, temp);
A.project(r);
// z = M^(-1) * r
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = this->dot(z,r);
if (r_dot_z <= Base::m_tolerance) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
}
return 0;
}
p = z;
btScalar r_dot_z_new = r_dot_z;
for (int k = 1; k <= Base::m_maxIterations; k++) {
// temp = A*p
A.multiply(p, temp);
A.project(temp);
if (this->dot(p,temp) < SIMD_EPSILON)
{
if (verbose)
std::cout << "Encountered negative direction in CG!" << std::endl;
if (k == 1)
{
x = b;
}
return k;
}
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z_new / this->dot(p, temp);
// x += alpha * p;
this->multAndAddTo(alpha, p, x);
// r -= alpha * temp;
this->multAndAddTo(-alpha, temp, r);
// z = M^(-1) * r
A.precondition(r, z);
r_dot_z = r_dot_z_new;
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;
}
return k;
}
btScalar beta = r_dot_z_new/r_dot_z; public:
p = this->multAndAdd(beta, p, z); btConjugateGradient(const int max_it_in)
} : btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
if (verbose) {
{ }
std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl;
} virtual ~btConjugateGradient() {}
return Base::m_maxIterations;
} // return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
void reinitialize(const TVStack& b) {
{ BT_PROFILE("CGSolve");
r.resize(b.size()); btAssert(x.size() == b.size());
p.resize(b.size()); reinitialize(b);
z.resize(b.size()); // r = b - A * x --with assigned dof zeroed out
temp.resize(b.size()); A.multiply(x, temp);
} r = this->sub(b, temp);
A.project(r);
// z = M^(-1) * r
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = this->dot(z, r);
if (r_dot_z <= Base::m_tolerance)
{
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
}
return 0;
}
p = z;
btScalar r_dot_z_new = r_dot_z;
for (int k = 1; k <= Base::m_maxIterations; k++)
{
// temp = A*p
A.multiply(p, temp);
A.project(temp);
if (this->dot(p, temp) < SIMD_EPSILON)
{
if (verbose)
std::cout << "Encountered negative direction in CG!" << std::endl;
if (k == 1)
{
x = b;
}
return k;
}
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z_new / this->dot(p, temp);
// x += alpha * p;
this->multAndAddTo(alpha, p, x);
// r -= alpha * temp;
this->multAndAddTo(-alpha, temp, r);
// z = M^(-1) * r
A.precondition(r, z);
r_dot_z = r_dot_z_new;
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;
}
return k;
}
btScalar beta = r_dot_z_new / r_dot_z;
p = this->multAndAdd(beta, p, z);
}
if (verbose)
{
std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl;
}
return Base::m_maxIterations;
}
void reinitialize(const TVStack& b)
{
r.resize(b.size());
p.resize(b.size());
z.resize(b.size());
temp.resize(b.size());
}
}; };
#endif /* btConjugateGradient_h */ #endif /* btConjugateGradient_h */

171
src/BulletSoftBody/btConjugateResidual.h
View File

@ -20,90 +20,93 @@
template <class MatrixX> template <class MatrixX>
class btConjugateResidual : public btKrylovSolver<MatrixX> class btConjugateResidual : public btKrylovSolver<MatrixX>
{ {
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base; typedef btKrylovSolver<MatrixX> Base;
TVStack r,p,z,temp_p, temp_r, best_x; TVStack r, p, z, temp_p, temp_r, best_x;
// temp_r = A*r // temp_r = A*r
// temp_p = A*p // temp_p = A*p
// z = M^(-1) * temp_p = M^(-1) * A * p // z = M^(-1) * temp_p = M^(-1) * A * p
btScalar best_r; btScalar best_r;
public: public:
btConjugateResidual(const int max_it_in) btConjugateResidual(const int max_it_in)
: Base(max_it_in, 1e-4) : Base(max_it_in, 1e-4)
{ {
} }
virtual ~btConjugateResidual(){} virtual ~btConjugateResidual() {}
// return the number of iterations taken // return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
{ {
BT_PROFILE("CRSolve"); BT_PROFILE("CRSolve");
btAssert(x.size() == b.size()); btAssert(x.size() == b.size());
reinitialize(b); reinitialize(b);
// r = b - A * x --with assigned dof zeroed out // r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp_r); // borrow temp_r here to store A*x A.multiply(x, temp_r); // borrow temp_r here to store A*x
r = this->sub(b, temp_r); r = this->sub(b, temp_r);
// z = M^(-1) * r // z = M^(-1) * r
A.precondition(r, z); // borrow z to store preconditioned r A.precondition(r, z); // borrow z to store preconditioned r
r = z; r = z;
btScalar residual_norm = this->norm(r); btScalar residual_norm = this->norm(r);
if (residual_norm <= Base::m_tolerance) { if (residual_norm <= Base::m_tolerance)
return 0; {
} return 0;
p = r; }
btScalar r_dot_Ar, r_dot_Ar_new; p = r;
// temp_p = A*p btScalar r_dot_Ar, r_dot_Ar_new;
A.multiply(p, temp_p); // temp_p = A*p
// temp_r = A*r A.multiply(p, temp_p);
temp_r = temp_p; // temp_r = A*r
r_dot_Ar = this->dot(r, temp_r); temp_r = temp_p;
for (int k = 1; k <= Base::m_maxIterations; k++) { r_dot_Ar = this->dot(r, temp_r);
// z = M^(-1) * Ap for (int k = 1; k <= Base::m_maxIterations; k++)
A.precondition(temp_p, z); {
// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) // z = M^(-1) * Ap
btScalar alpha = r_dot_Ar / this->dot(temp_p, z); A.precondition(temp_p, z);
// x += alpha * p; // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
this->multAndAddTo(alpha, p, x); btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
// r -= alpha * z; // x += alpha * p;
this->multAndAddTo(-alpha, z, r); this->multAndAddTo(alpha, p, x);
btScalar norm_r = this->norm(r); // r -= alpha * z;
if (norm_r < best_r) this->multAndAddTo(-alpha, z, r);
{ btScalar norm_r = this->norm(r);
best_x = x; if (norm_r < best_r)
best_r = norm_r; {
if (norm_r < Base::m_tolerance) { best_x = x;
return k; best_r = norm_r;
} if (norm_r < Base::m_tolerance)
} {
// temp_r = A * r; return k;
A.multiply(r, temp_r); }
r_dot_Ar_new = this->dot(r, temp_r); }
btScalar beta = r_dot_Ar_new/r_dot_Ar; // temp_r = A * r;
r_dot_Ar = r_dot_Ar_new; A.multiply(r, temp_r);
// p = beta*p + r; r_dot_Ar_new = this->dot(r, temp_r);
p = this->multAndAdd(beta, p, r); btScalar beta = r_dot_Ar_new / r_dot_Ar;
// temp_p = beta*temp_p + temp_r; r_dot_Ar = r_dot_Ar_new;
temp_p = this->multAndAdd(beta, temp_p, temp_r); // p = beta*p + r;
} p = this->multAndAdd(beta, p, r);
if (verbose) // temp_p = beta*temp_p + temp_r;
{ temp_p = this->multAndAdd(beta, temp_p, temp_r);
std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; }
} if (verbose)
x = best_x; {
return Base::m_maxIterations; std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
} }
x = best_x;
void reinitialize(const TVStack& b) return Base::m_maxIterations;
{ }
r.resize(b.size());
p.resize(b.size()); void reinitialize(const TVStack& b)
z.resize(b.size()); {
temp_p.resize(b.size()); r.resize(b.size());
temp_r.resize(b.size()); p.resize(b.size());
best_x.resize(b.size()); z.resize(b.size());
best_r = SIMD_INFINITY; temp_p.resize(b.size());
} temp_r.resize(b.size());
best_x.resize(b.size());
best_r = SIMD_INFINITY;
}
}; };
#endif /* btConjugateResidual_h */ #endif /* btConjugateResidual_h */

303
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -17,211 +17,208 @@
#include "btPreconditioner.h" #include "btPreconditioner.h"
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v) btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
: m_softBodies(softBodies) : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false)
, m_projection(softBodies)
, m_backupVelocity(backup_v)
, m_implicit(false)
{ {
m_massPreconditioner = new MassPreconditioner(m_softBodies); m_massPreconditioner = new MassPreconditioner(m_softBodies);
m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
m_preconditioner = m_KKTPreconditioner; m_preconditioner = m_KKTPreconditioner;
} }
btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
{ {
delete m_KKTPreconditioner; delete m_KKTPreconditioner;
delete m_massPreconditioner; delete m_massPreconditioner;
} }
void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
{ {
BT_PROFILE("reinitialize"); BT_PROFILE("reinitialize");
if (dt > 0) if (dt > 0)
{ {
setDt(dt); setDt(dt);
} }
if(nodeUpdated) if (nodeUpdated)
{ {
updateId(); updateId();
} }
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
{ {
m_lf[i]->reinitialize(nodeUpdated); m_lf[i]->reinitialize(nodeUpdated);
} }
m_projection.reinitialize(nodeUpdated); m_projection.reinitialize(nodeUpdated);
// m_preconditioner->reinitialize(nodeUpdated); // m_preconditioner->reinitialize(nodeUpdated);
} }
void btDeformableBackwardEulerObjective::setDt(btScalar dt) void btDeformableBackwardEulerObjective::setDt(btScalar dt)
{ {
m_dt = dt; m_dt = dt;
} }
void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
{ {
BT_PROFILE("multiply"); BT_PROFILE("multiply");
// add in the mass term // add in the mass term
size_t counter = 0; size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
const btSoftBody::Node& node = psb->m_nodes[j]; const btSoftBody::Node& node = psb->m_nodes[j];
b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im;
++counter; ++counter;
} }
} }
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
{ {
// add damping matrix // add damping matrix
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
if (m_implicit) if (m_implicit)
{ {
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b);
} }
} }
int offset = m_nodes.size(); int offset = m_nodes.size();
for (int i = offset; i < b.size(); ++i) for (int i = offset; i < b.size(); ++i)
{ {
b[i].setZero(); b[i].setZero();
} }
// add in the lagrange multiplier terms // add in the lagrange multiplier terms
for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
{ {
// C^T * lambda // C^T * lambda
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i) for (int i = 0; i < lm.m_num_nodes; ++i)
{ {
for (int j = 0; j < lm.m_num_constraints; ++j) for (int j = 0; j < lm.m_num_constraints; ++j)
{ {
b[lm.m_indices[i]] += x[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
} }
} }
// C * x // C * x
for (int d = 0; d < lm.m_num_constraints; ++d) for (int d = 0; d < lm.m_num_constraints; ++d)
{ {
for (int i = 0; i < lm.m_num_nodes; ++i) for (int i = 0; i < lm.m_num_nodes; ++i)
{ {
b[offset+c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
} }
} }
} }
} }
void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
{ {
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
btSoftBody::Node& node = psb->m_nodes[j]; btSoftBody::Node& node = psb->m_nodes[j];
node.m_v = m_backupVelocity[node.index] + dv[node.index]; node.m_v = m_backupVelocity[node.index] + dv[node.index];
} }
} }
} }
void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
{ {
size_t counter = 0; size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
counter += psb->m_nodes.size(); counter += psb->m_nodes.size();
continue; continue;
} }
for (int j = 0; j < psb->m_nodes.size(); ++j) 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; btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
psb->m_nodes[j].m_v += one_over_mass * force[counter++]; psb->m_nodes[j].m_v += one_over_mass * force[counter++];
} }
} }
if (setZero) if (setZero)
{ {
for (int i = 0; i < force.size(); ++i) for (int i = 0; i < force.size(); ++i)
force[i].setZero(); force[i].setZero();
} }
} }
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual)
{ {
BT_PROFILE("computeResidual"); BT_PROFILE("computeResidual");
// add implicit force // add implicit force
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
{ {
if (m_implicit) if (m_implicit)
{ {
m_lf[i]->addScaledForces(dt, residual); m_lf[i]->addScaledForces(dt, residual);
} }
else else
{ {
m_lf[i]->addScaledDampingForce(dt, residual); m_lf[i]->addScaledDampingForce(dt, residual);
} }
} }
// m_projection.project(residual); // m_projection.project(residual);
} }
btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
{ {
btScalar mag = 0; btScalar mag = 0;
for (int i = 0; i < residual.size(); ++i) for (int i = 0; i < residual.size(); ++i)
{ {
mag += residual[i].length2(); mag += residual[i].length2();
} }
return std::sqrt(mag); return std::sqrt(mag);
} }
btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt) btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
{ {
btScalar e = 0; btScalar e = 0;
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
{ {
e += m_lf[i]->totalEnergy(dt); e += m_lf[i]->totalEnergy(dt);
} }
return e; return e;
} }
void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
{ {
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
m_softBodies[i]->advanceDeformation(); m_softBodies[i]->advanceDeformation();
} }
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
{ {
m_lf[i]->addScaledExplicitForce(m_dt, force); m_lf[i]->addScaledExplicitForce(m_dt, force);
} }
applyForce(force, true); applyForce(force, true);
} }
void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
{ {
size_t counter = 0; size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
dv[counter] = psb->m_nodes[j].m_im * residual[counter]; dv[counter] = psb->m_nodes[j].m_im * residual[counter];
++counter; ++counter;
} }
} }
} }
//set constraints as projections //set constraints as projections
void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
{ {
m_projection.setConstraints(infoGlobal); m_projection.setConstraints(infoGlobal);
} }
void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
{ {
m_projection.applyDynamicFriction(r); m_projection.applyDynamicFriction(r);
} }

270
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@ -31,143 +31,143 @@
class btDeformableBackwardEulerObjective class btDeformableBackwardEulerObjective
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_dt; btScalar m_dt;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
btAlignedObjectArray<btSoftBody *>& m_softBodies; btAlignedObjectArray<btSoftBody*>& m_softBodies;
Preconditioner* m_preconditioner; Preconditioner* m_preconditioner;
btDeformableContactProjection m_projection; btDeformableContactProjection m_projection;
const TVStack& m_backupVelocity; const TVStack& m_backupVelocity;
btAlignedObjectArray<btSoftBody::Node* > m_nodes; btAlignedObjectArray<btSoftBody::Node*> m_nodes;
bool m_implicit; bool m_implicit;
MassPreconditioner* m_massPreconditioner; MassPreconditioner* m_massPreconditioner;
KKTPreconditioner* m_KKTPreconditioner; KKTPreconditioner* m_KKTPreconditioner;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v); btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
virtual ~btDeformableBackwardEulerObjective();
void initialize(){}
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
void computeResidual(btScalar dt, TVStack& residual);
// add explicit force to the velocity
void applyExplicitForce(TVStack& force);
// apply force to velocity and optionally reset the force to zero
void applyForce(TVStack& force, bool setZero);
// compute the norm of the residual
btScalar computeNorm(const TVStack& residual) const;
// compute one step of the solve (there is only one solve if the system is linear)
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
// perform A*x = b
void multiply(const TVStack& x, TVStack& b) const;
// set initial guess for CG solve
void initialGuess(TVStack& dv, const TVStack& residual);
// reset data structure and reset dt
void reinitialize(bool nodeUpdated, btScalar dt);
void setDt(btScalar dt);
// add friction force to residual
void applyDynamicFriction(TVStack& r);
// add dv to velocity
void updateVelocity(const TVStack& dv);
//set constraints as projections
void setConstraints(const btContactSolverInfo& infoGlobal);
// update the projections and project the residual
void project(TVStack& r)
{
BT_PROFILE("project");
m_projection.project(r);
}
// perform precondition M^(-1) x = b
void precondition(const TVStack& x, TVStack& b)
{
m_preconditioner->operator()(x,b);
}
// reindex all the vertices virtual ~btDeformableBackwardEulerObjective();
virtual void updateId()
{
size_t node_id = 0;
size_t face_id = 0;
m_nodes.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].index = node_id;
m_nodes.push_back(&psb->m_nodes[j]);
++node_id;
}
for (int j = 0; j < psb->m_faces.size(); ++j)
{
psb->m_faces[j].m_index = face_id;
++face_id;
}
}
}
const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
{
return &m_nodes;
}
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
// Calculate the total potential energy in the system void initialize() {}
btScalar totalEnergy(btScalar dt);
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) void computeResidual(btScalar dt, TVStack& residual);
{
extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); // add explicit force to the velocity
for (int i = 0; i < vec.size(); ++i) void applyExplicitForce(TVStack& force);
{
extended_vec[i] = vec[i]; // apply force to velocity and optionally reset the force to zero
} void applyForce(TVStack& force, bool setZero);
int offset = vec.size();
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) // compute the norm of the residual
{ btScalar computeNorm(const TVStack& residual) const;
extended_vec[offset + i].setZero();
} // compute one step of the solve (there is only one solve if the system is linear)
} void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) // perform A*x = b
{ void multiply(const TVStack& x, TVStack& b) const;
extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
for (int i = 0; i < residual.size(); ++i) // set initial guess for CG solve
{ void initialGuess(TVStack& dv, const TVStack& residual);
extended_residual[i] = residual[i];
} // reset data structure and reset dt
int offset = residual.size(); void reinitialize(bool nodeUpdated, btScalar dt);
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
{ void setDt(btScalar dt);
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
extended_residual[offset + i].setZero(); // add friction force to residual
for (int d = 0; d < lm.m_num_constraints; ++d) void applyDynamicFriction(TVStack& r);
{
for (int n = 0; n < lm.m_num_nodes; ++n) // add dv to velocity
{ void updateVelocity(const TVStack& dv);
extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
} //set constraints as projections
} void setConstraints(const btContactSolverInfo& infoGlobal);
}
} // update the projections and project the residual
void project(TVStack& r)
{
BT_PROFILE("project");
m_projection.project(r);
}
// perform precondition M^(-1) x = b
void precondition(const TVStack& x, TVStack& b)
{
m_preconditioner->operator()(x, b);
}
// reindex all the vertices
virtual void updateId()
{
size_t node_id = 0;
size_t face_id = 0;
m_nodes.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].index = node_id;
m_nodes.push_back(&psb->m_nodes[j]);
++node_id;
}
for (int j = 0; j < psb->m_faces.size(); ++j)
{
psb->m_faces[j].m_index = face_id;
++face_id;
}
}
}
const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
{
return &m_nodes;
}
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
// Calculate the total potential energy in the system
btScalar totalEnergy(btScalar dt);
void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
{
extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
for (int i = 0; i < vec.size(); ++i)
{
extended_vec[i] = vec[i];
}
int offset = vec.size();
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
{
extended_vec[offset + i].setZero();
}
}
void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
{
extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
for (int i = 0; i < residual.size(); ++i)
{
extended_residual[i] = residual[i];
}
int offset = residual.size();
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
{
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
extended_residual[offset + i].setZero();
for (int d = 0; d < lm.m_num_constraints; ++d)
{
for (int n = 0; n < lm.m_num_nodes; ++n)
{
extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
}
}
}
}
void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f) void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
{ {
@ -178,7 +178,7 @@ public:
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
const btSoftBody::Node& node = psb->m_nodes[j]; const btSoftBody::Node& node = psb->m_nodes[j];
f[counter] = (node.m_im == 0) ? btVector3(0,0,0) : dv[counter] / node.m_im; f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
++counter; ++counter;
} }
} }

715
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -20,462 +20,455 @@
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
static const int kMaxConjugateGradientIterations = 300; static const int kMaxConjugateGradientIterations = 300;
btDeformableBodySolver::btDeformableBodySolver() btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0) : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(5), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
, m_cg(kMaxConjugateGradientIterations)
, m_cr(kMaxConjugateGradientIterations)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
, m_lineSearch(false)
, m_useProjection(false)
{ {
m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
} }
btDeformableBodySolver::~btDeformableBodySolver() btDeformableBodySolver::~btDeformableBodySolver()
{ {
delete m_objective; delete m_objective;
} }
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{ {
BT_PROFILE("solveDeformableConstraints"); BT_PROFILE("solveDeformableConstraints");
if (!m_implicit) if (!m_implicit)
{ {
m_objective->computeResidual(solverdt, m_residual); m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual); m_objective->applyDynamicFriction(m_residual);
if (m_useProjection) if (m_useProjection)
{ {
computeStep(m_dv, m_residual); computeStep(m_dv, m_residual);
} }
else else
{ {
TVStack rhs, x; TVStack rhs, x;
m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
m_objective->addLagrangeMultiplier(m_dv, x); m_objective->addLagrangeMultiplier(m_dv, x);
m_objective->m_preconditioner->reinitialize(true); m_objective->m_preconditioner->reinitialize(true);
computeStep(x, rhs); computeStep(x, rhs);
for (int i = 0; i<m_dv.size(); ++i) for (int i = 0; i < m_dv.size(); ++i)
{ {
m_dv[i] = x[i]; m_dv[i] = x[i];
} }
} }
updateVelocity(); updateVelocity();
} }
else else
{ {
for (int i = 0; i < m_maxNewtonIterations; ++i) for (int i = 0; i < m_maxNewtonIterations; ++i)
{ {
updateState(); updateState();
// add the inertia term in the residual // add the inertia term in the residual
int counter = 0; int counter = 0;
for (int k = 0; k < m_softBodies.size(); ++k) for (int k = 0; k < m_softBodies.size(); ++k)
{ {
btSoftBody* psb = m_softBodies[k]; btSoftBody* psb = m_softBodies[k];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
if (psb->m_nodes[j].m_im > 0) if (psb->m_nodes[j].m_im > 0)
{ {
m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter]; m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
} }
++counter; ++counter;
} }
} }
m_objective->computeResidual(solverdt, m_residual); m_objective->computeResidual(solverdt, m_residual);
if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
{ {
break; break;
} }
// todo xuchenhan@: this really only needs to be calculated once // todo xuchenhan@: this really only needs to be calculated once
m_objective->applyDynamicFriction(m_residual); m_objective->applyDynamicFriction(m_residual);
if (m_lineSearch) if (m_lineSearch)
{ {
btScalar inner_product = computeDescentStep(m_ddv,m_residual); btScalar inner_product = computeDescentStep(m_ddv, m_residual);
btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
btScalar scale = 2; btScalar scale = 2;
btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2; btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
backupDv(); backupDv();
do { do
scale *= beta; {
if (scale < 1e-8) { scale *= beta;
return; if (scale < 1e-8)
} {
updateEnergy(scale); return;
f1 = m_objective->totalEnergy(solverdt)+kineticEnergy(); }
f2 = f0 - alpha * scale * inner_product; updateEnergy(scale);
} while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
revertDv(); f2 = f0 - alpha * scale * inner_product;
updateDv(scale); } while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues
} revertDv();
else updateDv(scale);
{ }
computeStep(m_ddv, m_residual); else
updateDv(); {
} computeStep(m_ddv, m_residual);
for (int j = 0; j < m_numNodes; ++j) updateDv();
{ }
m_ddv[j].setZero(); for (int j = 0; j < m_numNodes; ++j)
m_residual[j].setZero(); {
} m_ddv[j].setZero();
} m_residual[j].setZero();
updateVelocity(); }
} }
updateVelocity();
}
} }
btScalar btDeformableBodySolver::kineticEnergy() btScalar btDeformableBodySolver::kineticEnergy()
{ {
btScalar ke = 0; btScalar ke = 0;
for (int i = 0; i < m_softBodies.size();++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size();++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
btSoftBody::Node& node = psb->m_nodes[j]; btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0) if (node.m_im > 0)
{ {
ke += m_dv[node.index].length2() * 0.5 / node.m_im; ke += m_dv[node.index].length2() * 0.5 / node.m_im;
} }
} }
} }
return ke; return ke;
} }
void btDeformableBodySolver::backupDv() void btDeformableBodySolver::backupDv()
{ {
m_backup_dv.resize(m_dv.size()); m_backup_dv.resize(m_dv.size());
for (int i = 0; i<m_backup_dv.size(); ++i) for (int i = 0; i < m_backup_dv.size(); ++i)
{ {
m_backup_dv[i] = m_dv[i]; m_backup_dv[i] = m_dv[i];
} }
} }
void btDeformableBodySolver::revertDv() void btDeformableBodySolver::revertDv()
{ {
for (int i = 0; i<m_backup_dv.size(); ++i) for (int i = 0; i < m_backup_dv.size(); ++i)
{ {
m_dv[i] = m_backup_dv[i]; m_dv[i] = m_backup_dv[i];
} }
} }
void btDeformableBodySolver::updateEnergy(btScalar scale) void btDeformableBodySolver::updateEnergy(btScalar scale)
{ {
for (int i = 0; i<m_dv.size(); ++i) for (int i = 0; i < m_dv.size(); ++i)
{ {
m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
} }
updateState(); updateState();
} }
btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose) btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
{ {
m_cg.solve(*m_objective, ddv, residual, false); m_cg.solve(*m_objective, ddv, residual, false);
btScalar inner_product = m_cg.dot(residual, m_ddv); btScalar inner_product = m_cg.dot(residual, m_ddv);
btScalar res_norm = m_objective->computeNorm(residual); btScalar res_norm = m_objective->computeNorm(residual);
btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
if (inner_product < -tol) if (inner_product < -tol)
{ {
if (verbose) if (verbose)
{ {
std::cout << "Looking backwards!" << std::endl; std::cout << "Looking backwards!" << std::endl;
} }
for (int i = 0; i < m_ddv.size();++i) for (int i = 0; i < m_ddv.size(); ++i)
{ {
m_ddv[i] = -m_ddv[i]; m_ddv[i] = -m_ddv[i];
} }
inner_product = -inner_product; inner_product = -inner_product;
} }
else if (std::abs(inner_product) < tol) else if (std::abs(inner_product) < tol)
{ {
if (verbose) if (verbose)
{ {
std::cout << "Gradient Descent!" << std::endl; std::cout << "Gradient Descent!" << std::endl;
} }
btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
for (int i = 0; i < m_ddv.size();++i) for (int i = 0; i < m_ddv.size(); ++i)
{ {
m_ddv[i] = scale * residual[i]; m_ddv[i] = scale * residual[i];
} }
inner_product = scale * res_norm * res_norm; inner_product = scale * res_norm * res_norm;
} }
return inner_product; return inner_product;
} }
void btDeformableBodySolver::updateState() void btDeformableBodySolver::updateState()
{ {
updateVelocity(); updateVelocity();
updateTempPosition(); updateTempPosition();
} }
void btDeformableBodySolver::updateDv(btScalar scale) void btDeformableBodySolver::updateDv(btScalar scale)
{ {
for (int i = 0; i < m_numNodes; ++i) for (int i = 0; i < m_numNodes; ++i)
{ {
m_dv[i] += scale * m_ddv[i]; m_dv[i] += scale * m_ddv[i];
} }
} }
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{ {
if (m_useProjection) if (m_useProjection)
m_cg.solve(*m_objective, ddv, residual, false); m_cg.solve(*m_objective, ddv, residual, false);
else else
m_cr.solve(*m_objective, ddv, residual, false); m_cr.solve(*m_objective, ddv, residual, false);
} }
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt) void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
{ {
m_softBodies.copyFromArray(softBodies); m_softBodies.copyFromArray(softBodies);
bool nodeUpdated = updateNodes(); bool nodeUpdated = updateNodes();
if (nodeUpdated) if (nodeUpdated)
{ {
m_dv.resize(m_numNodes, btVector3(0,0,0)); m_dv.resize(m_numNodes, btVector3(0, 0, 0));
m_ddv.resize(m_numNodes, btVector3(0,0,0)); m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
m_residual.resize(m_numNodes, btVector3(0,0,0)); m_residual.resize(m_numNodes, btVector3(0, 0, 0));
m_backupVelocity.resize(m_numNodes, btVector3(0,0,0)); m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
} }
// need to setZero here as resize only set value for newly allocated items // need to setZero here as resize only set value for newly allocated items
for (int i = 0; i < m_numNodes; ++i) for (int i = 0; i < m_numNodes; ++i)
{ {
m_dv[i].setZero(); m_dv[i].setZero();
m_ddv[i].setZero(); m_ddv[i].setZero();
m_residual[i].setZero(); m_residual[i].setZero();
} }
m_dt = dt; m_dt = dt;
m_objective->reinitialize(nodeUpdated, dt); m_objective->reinitialize(nodeUpdated, dt);
updateSoftBodies(); updateSoftBodies();
} }
void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
{ {
BT_PROFILE("setConstraint"); BT_PROFILE("setConstraint");
m_objective->setConstraints(infoGlobal); m_objective->setConstraints(infoGlobal);
} }
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{ {
BT_PROFILE("solveContactConstraints"); BT_PROFILE("solveContactConstraints");
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
return maxSquaredResidual; return maxSquaredResidual;
} }
void btDeformableBodySolver::updateVelocity() void btDeformableBodySolver::updateVelocity()
{ {
int counter = 0; int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
psb->m_maxSpeedSquared = 0; psb->m_maxSpeedSquared = 0;
if (!psb->isActive()) if (!psb->isActive())
{ {
counter += psb->m_nodes.size(); counter += psb->m_nodes.size();
continue; continue;
} }
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
// set NaN to zero; // set NaN to zero;
if (m_dv[counter] != m_dv[counter]) if (m_dv[counter] != m_dv[counter])
{ {
m_dv[counter].setZero(); m_dv[counter].setZero();
} }
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; 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()); psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter; ++counter;
} }
} }
} }
void btDeformableBodySolver::updateTempPosition() void btDeformableBodySolver::updateTempPosition()
{ {
int counter = 0; int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
counter += psb->m_nodes.size(); counter += psb->m_nodes.size();
continue; continue;
} }
for (int j = 0; j < psb->m_nodes.size(); ++j) 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; psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
++counter; ++counter;
} }
psb->updateDeformation(); psb->updateDeformation();
} }
} }
void btDeformableBodySolver::backupVelocity() void btDeformableBodySolver::backupVelocity()
{ {
int counter = 0; int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
m_backupVelocity[counter++] = psb->m_nodes[j].m_v; m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
} }
} }
} }
void btDeformableBodySolver::setupDeformableSolve(bool implicit) void btDeformableBodySolver::setupDeformableSolve(bool implicit)
{ {
int counter = 0; int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
counter += psb->m_nodes.size(); counter += psb->m_nodes.size();
continue; continue;
} }
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
if (implicit) if (implicit)
{ {
if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON) 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]; m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
else else
m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
m_backupVelocity[counter] = psb->m_nodes[j].m_vn; m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
} }
else else
{ {
m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - 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]; psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter; ++counter;
} }
} }
} }
void btDeformableBodySolver::revertVelocity() void btDeformableBodySolver::revertVelocity()
{ {
int counter = 0; int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
psb->m_nodes[j].m_v = m_backupVelocity[counter++]; psb->m_nodes[j].m_v = m_backupVelocity[counter++];
} }
} }
} }
bool btDeformableBodySolver::updateNodes() bool btDeformableBodySolver::updateNodes()
{ {
int numNodes = 0; int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
numNodes += m_softBodies[i]->m_nodes.size(); numNodes += m_softBodies[i]->m_nodes.size();
if (numNodes != m_numNodes) if (numNodes != m_numNodes)
{ {
m_numNodes = numNodes; m_numNodes = numNodes;
return true; return true;
} }
return false; return false;
} }
void btDeformableBodySolver::predictMotion(btScalar solverdt) void btDeformableBodySolver::predictMotion(btScalar solverdt)
{ {
// apply explicit forces to velocity // apply explicit forces to velocity
m_objective->applyExplicitForce(m_residual); m_objective->applyExplicitForce(m_residual);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody *psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (psb->isActive()) if (psb->isActive())
{ {
// predict motion for collision detection // predict motion for collision detection
predictDeformableMotion(psb, solverdt); predictDeformableMotion(psb, solverdt);
} }
} }
} }
void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
{ {
BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
int i, ni; int i, ni;
/* Update */
if (psb->m_bUpdateRtCst)
{
psb->m_bUpdateRtCst = false;
psb->updateConstants();
psb->m_fdbvt.clear();
if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
{
psb->initializeFaceTree();
}
}
/* Prepare */
psb->m_sst.sdt = dt * psb->m_cfg.timescale;
psb->m_sst.isdt = 1 / psb->m_sst.sdt;
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;
n.m_splitv.setZero();
n.m_constrained = false;
}
/* Nodes */ /* Update */
psb->updateNodeTree(true, true); if (psb->m_bUpdateRtCst)
if (!psb->m_fdbvt.empty()) {
{ psb->m_bUpdateRtCst = false;
psb->updateFaceTree(true, true); psb->updateConstants();
} psb->m_fdbvt.clear();
/* Clear contacts */ if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
psb->m_nodeRigidContacts.resize(0); {
psb->m_faceRigidContacts.resize(0); psb->initializeFaceTree();
psb->m_faceNodeContacts.resize(0); }
/* Optimize dbvt's */ }
// psb->m_ndbvt.optimizeIncremental(1);
// psb->m_fdbvt.optimizeIncremental(1); /* Prepare */
psb->m_sst.sdt = dt * psb->m_cfg.timescale;
psb->m_sst.isdt = 1 / psb->m_sst.sdt;
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;
n.m_splitv.setZero();
n.m_constrained = false;
}
/* Nodes */
psb->updateNodeTree(true, true);
if (!psb->m_fdbvt.empty())
{
psb->updateFaceTree(true, true);
}
/* Clear contacts */
psb->m_nodeRigidContacts.resize(0);
psb->m_faceRigidContacts.resize(0);
psb->m_faceNodeContacts.resize(0);
/* Optimize dbvt's */
// psb->m_ndbvt.optimizeIncremental(1);
// psb->m_fdbvt.optimizeIncremental(1);
} }
void btDeformableBodySolver::updateSoftBodies() void btDeformableBodySolver::updateSoftBodies()
{ {
BT_PROFILE("updateSoftBodies"); BT_PROFILE("updateSoftBodies");
for (int i = 0; i < m_softBodies.size(); i++) for (int i = 0; i < m_softBodies.size(); i++)
{ {
btSoftBody *psb = (btSoftBody *)m_softBodies[i]; btSoftBody* psb = (btSoftBody*)m_softBodies[i];
if (psb->isActive()) if (psb->isActive())
{ {
psb->updateNormals(); psb->updateNormals();
} }
} }
} }
void btDeformableBodySolver::setImplicit(bool implicit) void btDeformableBodySolver::setImplicit(bool implicit)
{ {
m_implicit = implicit; m_implicit = implicit;
m_objective->setImplicit(implicit); m_objective->setImplicit(implicit);
} }
void btDeformableBodySolver::setLineSearch(bool lineSearch) void btDeformableBodySolver::setLineSearch(bool lineSearch)
{ {
m_lineSearch = lineSearch; m_lineSearch = lineSearch;
} }

237
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -16,7 +16,6 @@
#ifndef BT_DEFORMABLE_BODY_SOLVERS_H #ifndef BT_DEFORMABLE_BODY_SOLVERS_H
#define BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H
#include "btSoftBodySolvers.h" #include "btSoftBodySolvers.h"
#include "btDeformableBackwardEulerObjective.h" #include "btDeformableBackwardEulerObjective.h"
#include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableMultiBodyDynamicsWorld.h"
@ -30,130 +29,132 @@ class btDeformableMultiBodyDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver class btDeformableBodySolver : public btSoftBodySolver
{ {
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
protected: protected:
int m_numNodes; // total number of deformable body nodes int m_numNodes; // total number of deformable body nodes
TVStack m_dv; // v_{n+1} - v_n TVStack m_dv; // v_{n+1} - v_n
TVStack m_backup_dv; // backed up dv TVStack m_backup_dv; // backed up dv
TVStack m_ddv; // incremental dv TVStack m_ddv; // incremental dv
TVStack m_residual; // rhs of the linear solve TVStack m_residual; // rhs of the linear solve
btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
btScalar m_dt; // dt btScalar m_dt; // dt
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver
bool m_implicit; // use implicit scheme if true, explicit scheme if false bool m_implicit; // use implicit scheme if true, explicit scheme if false
int m_maxNewtonIterations; // max number of newton iterations int m_maxNewtonIterations; // max number of newton iterations
btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance
bool m_lineSearch; // If true, use newton's method with line search under implicit scheme bool m_lineSearch; // If true, use newton's method with line search under implicit scheme
public: public:
// handles data related to objective function // handles data related to objective function
btDeformableBackwardEulerObjective* m_objective; btDeformableBackwardEulerObjective* m_objective;
bool m_useProjection; bool m_useProjection;
btDeformableBodySolver();
virtual ~btDeformableBodySolver();
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// update soft body normals btDeformableBodySolver();
virtual void updateSoftBodies();
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// resize/clear data structures virtual ~btDeformableBodySolver();
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
// set up contact constraints
void setConstraints(const btContactSolverInfo& infoGlobal);
// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
virtual void predictMotion(btScalar solverdt);
// move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
// x_{n+1}^* is stored in m_q
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
// save the current velocity to m_backupVelocity
void backupVelocity();
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
void setupDeformableSolve(bool implicit);
// set the current velocity to that backed up in m_backupVelocity
void revertVelocity();
// set velocity to m_dv + m_backupVelocity
void updateVelocity();
// update the node count
bool updateNodes();
// calculate the change in dv resulting from the momentum solve
void computeStep(TVStack& ddv, const TVStack& residual);
// calculate the change in dv resulting from the momentum solve when line search is turned on
btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false);
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// process collision between deformable and rigid // update soft body normals
virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) virtual void updateSoftBodies();
{
softBody->defaultCollisionHandler(collisionObjectWrap);
}
// process collision between deformable and deformable
virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
softBody->defaultCollisionHandler(otherSoftBody);
}
// If true, implicit time stepping scheme is used. virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// Otherwise, explicit time stepping scheme is used
void setImplicit(bool implicit); // solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// If true, newton's method with line search is used when implicit time stepping scheme is turned on
void setLineSearch(bool lineSearch); // resize/clear data structures
void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
// set temporary position x^* = x_n + dt * v
// update the deformation gradient at position x^* // set up contact constraints
void updateState(); void setConstraints(const btContactSolverInfo& infoGlobal);
// set dv = dv + scale * ddv // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
void updateDv(btScalar scale = 1); virtual void predictMotion(btScalar solverdt);
// set temporary position x^* = x_n + dt * v^* // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
void updateTempPosition(); // x_{n+1}^* is stored in m_q
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
// save the current dv to m_backup_dv;
void backupDv(); // save the current velocity to m_backupVelocity
void backupVelocity();
// set dv to the backed-up value
void revertDv(); // set m_dv and m_backupVelocity to desired value to prepare for momentum solve
void setupDeformableSolve(bool implicit);
// set dv = dv + scale * ddv
// set v^* = v_n + dv // set the current velocity to that backed up in m_backupVelocity
// set temporary position x^* = x_n + dt * v^* void revertVelocity();
// update the deformation gradient at position x^*
void updateEnergy(btScalar scale); // set velocity to m_dv + m_backupVelocity
void updateVelocity();
// calculates the appropriately scaled kinetic energy in the system, which is
// 1/2 * dv^T * M * dv // update the node count
// used in line search bool updateNodes();
btScalar kineticEnergy();
// calculate the change in dv resulting from the momentum solve
// unused functions void computeStep(TVStack& ddv, const TVStack& residual);
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
virtual void solveConstraints(btScalar dt){} // calculate the change in dv resulting from the momentum solve when line search is turned on
virtual bool checkInitialized(){return true;} btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false);
virtual void copyBackToSoftBodies(bool bMove = true) {}
virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {}
// process collision between deformable and rigid
virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap)
{
softBody->defaultCollisionHandler(collisionObjectWrap);
}
// process collision between deformable and deformable
virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody)
{
softBody->defaultCollisionHandler(otherSoftBody);
}
// If true, implicit time stepping scheme is used.
// Otherwise, explicit time stepping scheme is used
void setImplicit(bool implicit);
// If true, newton's method with line search is used when implicit time stepping scheme is turned on
void setLineSearch(bool lineSearch);
// set temporary position x^* = x_n + dt * v
// update the deformation gradient at position x^*
void updateState();
// set dv = dv + scale * ddv
void updateDv(btScalar scale = 1);
// set temporary position x^* = x_n + dt * v^*
void updateTempPosition();
// save the current dv to m_backup_dv;
void backupDv();
// set dv to the backed-up value
void revertDv();
// set dv = dv + scale * ddv
// set v^* = v_n + dv
// set temporary position x^* = x_n + dt * v^*
// update the deformation gradient at position x^*
void updateEnergy(btScalar scale);
// calculates the appropriately scaled kinetic energy in the system, which is
// 1/2 * dv^T * M * dv
// used in line search
btScalar kineticEnergy();
// unused functions
virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
virtual void solveConstraints(btScalar dt) {}
virtual bool checkInitialized() { return true; }
virtual void copyBackToSoftBodies(bool bMove = true) {}
}; };
#endif /* btDeformableBodySolver_h */ #endif /* btDeformableBodySolver_h */

980
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

File diff suppressed because it is too large Load Diff

398
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -21,51 +21,49 @@
class btDeformableContactConstraint class btDeformableContactConstraint
{ {
public: public:
// True if the friction is static // True if the friction is static
// False if the friction is dynamic // False if the friction is dynamic
bool m_static; bool m_static;
const btContactSolverInfo* m_infoGlobal; const btContactSolverInfo* m_infoGlobal;
// normal of the contact // normal of the contact
btVector3 m_normal; btVector3 m_normal;
btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
{ {
} }
btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
{ {
} }
btDeformableContactConstraint(){} btDeformableContactConstraint() {}
btDeformableContactConstraint(const btDeformableContactConstraint& other) btDeformableContactConstraint(const btDeformableContactConstraint& other)
: m_static(other.m_static) : m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal)
, m_normal(other.m_normal)
, m_infoGlobal(other.m_infoGlobal)
{ {
} }
virtual ~btDeformableContactConstraint(){} virtual ~btDeformableContactConstraint() {}
// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
// get the velocity of the object A in the contact // get the velocity of the object A in the contact
virtual btVector3 getVa() const = 0; virtual btVector3 getVa() const = 0;
// get the velocity of the object B in the contact // get the velocity of the object B in the contact
virtual btVector3 getVb() const = 0; virtual btVector3 getVb() const = 0;
// get the velocity change of the soft body node in the constraint // get the velocity change of the soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const = 0; virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
// apply impulse to the soft body node and/or face involved // apply impulse to the soft body node and/or face involved
virtual void applyImpulse(const btVector3& impulse) = 0; virtual void applyImpulse(const btVector3& impulse) = 0;
// scale the penetration depth by erp // scale the penetration depth by erp
virtual void setPenetrationScale(btScalar scale) = 0; virtual void setPenetrationScale(btScalar scale) = 0;
}; };
// //
@ -73,42 +71,41 @@ public:
class btDeformableStaticConstraint : public btDeformableContactConstraint class btDeformableStaticConstraint : public btDeformableContactConstraint
{ {
public: public:
btSoftBody::Node* m_node; btSoftBody::Node* m_node;
btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
{
}
btDeformableStaticConstraint(){}
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
: m_node(other.m_node)
, btDeformableContactConstraint(other)
{
}
virtual ~btDeformableStaticConstraint(){}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
{
return 0;
}
virtual btVector3 getVa() const btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal)
{ {
return btVector3(0,0,0); }
} btDeformableStaticConstraint() {}
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
virtual btVector3 getVb() const : m_node(other.m_node), btDeformableContactConstraint(other)
{ {
return btVector3(0,0,0); }
}
virtual ~btDeformableStaticConstraint() {}
virtual btVector3 getDv(const btSoftBody::Node* n) const
{ virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
return btVector3(0,0,0); {
} return 0;
}
virtual void applyImpulse(const btVector3& impulse){}
virtual void setPenetrationScale(btScalar scale){} virtual btVector3 getVa() const
{
return btVector3(0, 0, 0);
}
virtual btVector3 getVb() const
{
return btVector3(0, 0, 0);
}
virtual btVector3 getDv(const btSoftBody::Node* n) const
{
return btVector3(0, 0, 0);
}
virtual void applyImpulse(const btVector3& impulse) {}
virtual void setPenetrationScale(btScalar scale) {}
}; };
// //
@ -116,68 +113,67 @@ public:
class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
{ {
public: public:
const btSoftBody::DeformableNodeRigidAnchor* m_anchor; const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
btDeformableNodeAnchorConstraint(){}
virtual ~btDeformableNodeAnchorConstraint()
{
}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
// object A is the rigid/multi body, and object B is the deformable node/face btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
virtual btVector3 getVa() const; btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
// get the velocity of the deformable node in contact btDeformableNodeAnchorConstraint() {}
virtual btVector3 getVb() const; virtual ~btDeformableNodeAnchorConstraint()
virtual btVector3 getDv(const btSoftBody::Node* n) const {
{ }
return btVector3(0,0,0); virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void setPenetrationScale(btScalar scale){} // 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);
virtual void setPenetrationScale(btScalar scale) {}
}; };
// //
// Constraint between rigid/multi body and deformable objects // Constraint between rigid/multi body and deformable objects
class btDeformableRigidContactConstraint : public btDeformableContactConstraint class btDeformableRigidContactConstraint : public btDeformableContactConstraint
{ {
public: public:
btVector3 m_total_normal_dv; btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv; btVector3 m_total_tangent_dv;
btScalar m_penetration; btScalar m_penetration;
btScalar m_total_split_impulse; btScalar m_total_split_impulse;
bool m_binding; bool m_binding;
const btSoftBody::DeformableRigidContact* m_contact; const btSoftBody::DeformableRigidContact* m_contact;
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
btDeformableRigidContactConstraint(){} btDeformableRigidContactConstraint() {}
virtual ~btDeformableRigidContactConstraint() virtual ~btDeformableRigidContactConstraint()
{ {
} }
// object A is the rigid/multi body, and object B is the deformable node/face // object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const; virtual btVector3 getVa() const;
// get the split impulse velocity of the deformable face at the contact point // get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const = 0; virtual btVector3 getSplitVb() const = 0;
// get the split impulse velocity of the rigid/multibdoy at the contaft // get the split impulse velocity of the rigid/multibdoy at the contaft
virtual btVector3 getSplitVa() const; virtual btVector3 getSplitVa() const;
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale) virtual void setPenetrationScale(btScalar scale)
{ {
m_penetration *= scale; m_penetration *= scale;
} }
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
virtual void applySplitImpulse(const btVector3& impulse) = 0; virtual void applySplitImpulse(const btVector3& impulse) = 0;
}; };
// //
@ -185,34 +181,34 @@ public:
class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
{ {
public: public:
// the deformable node in contact // the deformable node in contact
btSoftBody::Node* m_node; btSoftBody::Node* m_node;
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
btDeformableNodeRigidContactConstraint(){} btDeformableNodeRigidContactConstraint() {}
virtual ~btDeformableNodeRigidContactConstraint() virtual ~btDeformableNodeRigidContactConstraint()
{ {
} }
// get the velocity of the deformable node in contact // get the velocity of the deformable node in contact
virtual btVector3 getVb() const; virtual btVector3 getVb() const;
// get the split impulse velocity of the deformable face at the contact point // get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const; virtual btVector3 getSplitVb() const;
// get the velocity change of the input soft body node in the constraint // get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const; virtual btVector3 getDv(const btSoftBody::Node*) const;
// cast the contact to the desired type // cast the contact to the desired type
const btSoftBody::DeformableNodeRigidContact* getContact() const const btSoftBody::DeformableNodeRigidContact* getContact() const
{ {
return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
} }
virtual void applyImpulse(const btVector3& impulse); virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse); virtual void applySplitImpulse(const btVector3& impulse);
}; };
// //
@ -220,33 +216,33 @@ public:
class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
{ {
public: public:
const btSoftBody::Face* m_face; const btSoftBody::Face* m_face;
bool m_useStrainLimiting; bool m_useStrainLimiting;
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {} btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {}
virtual ~btDeformableFaceRigidContactConstraint() virtual ~btDeformableFaceRigidContactConstraint()
{ {
} }
// 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 // get the velocity of the deformable face at the contact point
const btSoftBody::DeformableFaceRigidContact* getContact() const virtual btVector3 getVb() const;
{
return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); // get the split impulse velocity of the deformable face at the contact point
} virtual btVector3 getSplitVb() const;
virtual void applyImpulse(const btVector3& impulse); // get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
virtual void applySplitImpulse(const btVector3& impulse);
// cast the contact to the desired type
const btSoftBody::DeformableFaceRigidContact* getContact() const
{
return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
}; };
// //
@ -254,35 +250,35 @@ public:
class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint
{ {
public: public:
btSoftBody::Node* m_node; btSoftBody::Node* m_node;
btSoftBody::Face* m_face; btSoftBody::Face* m_face;
const btSoftBody::DeformableFaceNodeContact* m_contact; const btSoftBody::DeformableFaceNodeContact* m_contact;
btVector3 m_total_normal_dv; btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv; btVector3 m_total_tangent_dv;
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
btDeformableFaceNodeContactConstraint(){}
virtual ~btDeformableFaceNodeContactConstraint(){}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
// get the velocity of the object A in the contact
virtual btVector3 getVa() const;
// get the velocity of the object B in the contact
virtual btVector3 getVb() 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::DeformableFaceNodeContact* getContact() const
{
return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void setPenetrationScale(btScalar scale){} btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
btDeformableFaceNodeContactConstraint() {}
virtual ~btDeformableFaceNodeContactConstraint() {}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
// get the velocity of the object A in the contact
virtual btVector3 getVa() const;
// get the velocity of the object B in the contact
virtual btVector3 getVb() 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::DeformableFaceNodeContact* getContact() const
{
return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void setPenetrationScale(btScalar scale) {}
}; };
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

749
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -17,7 +17,7 @@
#include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{ {
btScalar residualSquare = 0; btScalar residualSquare = 0;
for (int i = 0; i < numDeformableBodies; ++i) for (int i = 0; i < numDeformableBodies; ++i)
@ -58,31 +58,31 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
return residualSquare; return residualSquare;
} }
btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{ {
btScalar residualSquare = 0; btScalar residualSquare = 0;
for (int i = 0; i < numDeformableBodies; ++i) for (int i = 0; i < numDeformableBodies; ++i)
{ {
for (int j = 0; j < m_softBodies.size(); ++j) for (int j = 0; j < m_softBodies.size(); ++j)
{ {
btCollisionObject* psb = m_softBodies[j]; btCollisionObject* psb = m_softBodies[j];
if (psb != deformableBodies[i]) if (psb != deformableBodies[i])
{ {
continue; continue;
} }
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
{ {
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare); residualSquare = btMax(residualSquare, localResidualSquare);
} }
} }
} }
return residualSquare; return residualSquare;
} }
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
{ {
BT_PROFILE("setConstraints"); BT_PROFILE("setConstraints");
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
@ -101,7 +101,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
m_staticConstraints[i].push_back(static_constraint); m_staticConstraints[i].push_back(static_constraint);
} }
} }
// set up deformable anchors // set up deformable anchors
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{ {
@ -115,7 +115,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
m_nodeAnchorConstraints[i].push_back(constraint); m_nodeAnchorConstraints[i].push_back(constraint);
} }
// set Deformable Node vs. Rigid constraint // set Deformable Node vs. Rigid constraint
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{ {
@ -128,7 +128,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
m_nodeRigidConstraints[i].push_back(constraint); m_nodeRigidConstraints[i].push_back(constraint);
} }
// set Deformable Face vs. Rigid constraint // set Deformable Face vs. Rigid constraint
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
{ {
@ -139,16 +139,16 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
continue; continue;
} }
btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
m_faceRigidConstraints[i].push_back(constraint); m_faceRigidConstraints[i].push_back(constraint);
// btVector3 va = constraint.getVa(); // btVector3 va = constraint.getVa();
// btVector3 vb = constraint.getVb(); // btVector3 vb = constraint.getVb();
// const btVector3 vr = vb - va; // const btVector3 vr = vb - va;
// const btSoftBody::sCti& cti = contact.m_cti; // const btSoftBody::sCti& cti = contact.m_cti;
// const btScalar dn = btDot(vr, cti.m_normal); // const btScalar dn = btDot(vr, cti.m_normal);
// if (dn < SIMD_EPSILON) // if (dn < SIMD_EPSILON)
// { // {
// m_faceRigidConstraints[i].push_back(constraint); // m_faceRigidConstraints[i].push_back(constraint);
// } // }
} }
} }
} }
@ -156,268 +156,267 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
void btDeformableContactProjection::project(TVStack& x) void btDeformableContactProjection::project(TVStack& x)
{ {
#ifndef USE_MGS #ifndef USE_MGS
const int dim = 3; const int dim = 3;
for (int index = 0; index < m_projectionsDict.size(); ++index) for (int index = 0; index < m_projectionsDict.size(); ++index)
{ {
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
if (projectionDirs.size() >= dim) if (projectionDirs.size() >= dim)
{ {
// static node // static node
x[i].setZero(); x[i].setZero();
continue; continue;
} }
else if (projectionDirs.size() == 2) else if (projectionDirs.size() == 2)
{ {
btVector3 dir0 = projectionDirs[0]; btVector3 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1]; btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1); btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.safeNorm() < SIMD_EPSILON) if (free_dir.safeNorm() < SIMD_EPSILON)
{ {
x[i] -= x[i].dot(dir0) * dir0; x[i] -= x[i].dot(dir0) * dir0;
x[i] -= x[i].dot(dir1) * dir1; x[i] -= x[i].dot(dir1) * dir1;
} }
else else
{ {
free_dir.normalize(); free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir; x[i] = x[i].dot(free_dir) * free_dir;
} }
} }
else else
{ {
btAssert(projectionDirs.size() == 1); btAssert(projectionDirs.size() == 1);
btVector3 dir0 = projectionDirs[0]; btVector3 dir0 = projectionDirs[0];
x[i] -= x[i].dot(dir0) * dir0; x[i] -= x[i].dot(dir0) * dir0;
} }
} }
#else #else
btReducedVector p(x.size()); btReducedVector p(x.size());
for (int i = 0; i < m_projections.size(); ++i) for (int i = 0; i < m_projections.size(); ++i)
{ {
p += (m_projections[i].dot(x) * m_projections[i]); p += (m_projections[i].dot(x) * m_projections[i]);
} }
for (int i = 0; i < p.m_indices.size(); ++i) for (int i = 0; i < p.m_indices.size(); ++i)
{ {
x[p.m_indices[i]] -= p.m_vecs[i]; x[p.m_indices[i]] -= p.m_vecs[i];
} }
#endif #endif
} }
void btDeformableContactProjection::setProjection() void btDeformableContactProjection::setProjection()
{ {
#ifndef USE_MGS #ifndef USE_MGS
BT_PROFILE("btDeformableContactProjection::setProjection"); BT_PROFILE("btDeformableContactProjection::setProjection");
btAlignedObjectArray<btVector3> units; btAlignedObjectArray<btVector3> units;
units.push_back(btVector3(1,0,0)); units.push_back(btVector3(1, 0, 0));
units.push_back(btVector3(0,1,0)); units.push_back(btVector3(0, 1, 0));
units.push_back(btVector3(0,0,1)); units.push_back(btVector3(0, 0, 1));
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < m_staticConstraints[i].size(); ++j) for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{ {
int index = m_staticConstraints[i][j].m_node->index; int index = m_staticConstraints[i][j].m_node->index;
m_staticConstraints[i][j].m_node->m_constrained = true; m_staticConstraints[i][j].m_node->m_constrained = true;
if (m_projectionsDict.find(index) == NULL) if (m_projectionsDict.find(index) == NULL)
{ {
m_projectionsDict.insert(index, units); m_projectionsDict.insert(index, units);
} }
else else
{ {
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
projections.push_back(units[k]); projections.push_back(units[k]);
} }
} }
} }
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{ {
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
if (m_projectionsDict.find(index) == NULL) if (m_projectionsDict.find(index) == NULL)
{ {
m_projectionsDict.insert(index, units); m_projectionsDict.insert(index, units);
} }
else else
{ {
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
projections.push_back(units[k]); projections.push_back(units[k]);
} }
} }
} }
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{ {
int index = m_nodeRigidConstraints[i][j].m_node->index; int index = m_nodeRigidConstraints[i][j].m_node->index;
m_nodeRigidConstraints[i][j].m_node->m_constrained = true; m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
if (m_nodeRigidConstraints[i][j].m_static) if (m_nodeRigidConstraints[i][j].m_static)
{ {
if (m_projectionsDict.find(index) == NULL) if (m_projectionsDict.find(index) == NULL)
{ {
m_projectionsDict.insert(index, units); m_projectionsDict.insert(index, units);
} }
else else
{ {
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
projections.push_back(units[k]); projections.push_back(units[k]);
} }
} }
} }
else else
{ {
if (m_projectionsDict.find(index) == NULL) if (m_projectionsDict.find(index) == NULL)
{ {
btAlignedObjectArray<btVector3> projections; btAlignedObjectArray<btVector3> projections;
projections.push_back(m_nodeRigidConstraints[i][j].m_normal); projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections); m_projectionsDict.insert(index, projections);
} }
else else
{ {
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_nodeRigidConstraints[i][j].m_normal); projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
} }
} }
} }
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{ {
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
if (m_faceRigidConstraints[i][j].m_binding) if (m_faceRigidConstraints[i][j].m_binding)
{ {
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
face->m_n[k]->m_constrained = true; face->m_n[k]->m_constrained = true;
} }
} }
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
btSoftBody::Node* node = face->m_n[k]; btSoftBody::Node* node = face->m_n[k];
int index = node->index; int index = node->index;
if (m_faceRigidConstraints[i][j].m_static) if (m_faceRigidConstraints[i][j].m_static)
{ {
if (m_projectionsDict.find(index) == NULL) if (m_projectionsDict.find(index) == NULL)
{ {
m_projectionsDict.insert(index, units); m_projectionsDict.insert(index, units);
} }
else else
{ {
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
projections.push_back(units[k]); projections.push_back(units[k]);
} }
} }
} }
else else
{ {
if (m_projectionsDict.find(index) == NULL) if (m_projectionsDict.find(index) == NULL)
{ {
btAlignedObjectArray<btVector3> projections; btAlignedObjectArray<btVector3> projections;
projections.push_back(m_faceRigidConstraints[i][j].m_normal); projections.push_back(m_faceRigidConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections); m_projectionsDict.insert(index, projections);
} }
else else
{ {
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_faceRigidConstraints[i][j].m_normal); projections.push_back(m_faceRigidConstraints[i][j].m_normal);
} }
} }
} }
} }
} }
#else #else
int dof = 0; int dof = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
dof += m_softBodies[i]->m_nodes.size(); dof += m_softBodies[i]->m_nodes.size();
} }
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < m_staticConstraints[i].size(); ++j) for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{ {
int index = m_staticConstraints[i][j].m_node->index; int index = m_staticConstraints[i][j].m_node->index;
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
btAlignedObjectArray<int> indices; btAlignedObjectArray<int> indices;
btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
indices.push_back(index); indices.push_back(index);
vecs1.push_back(btVector3(1,0,0)); vecs1.push_back(btVector3(1, 0, 0));
vecs2.push_back(btVector3(0,1,0)); vecs2.push_back(btVector3(0, 1, 0));
vecs3.push_back(btVector3(0,0,1)); vecs3.push_back(btVector3(0, 0, 1));
m_projections.push_back(btReducedVector(dof, indices, vecs1)); m_projections.push_back(btReducedVector(dof, indices, vecs1));
m_projections.push_back(btReducedVector(dof, indices, vecs2)); m_projections.push_back(btReducedVector(dof, indices, vecs2));
m_projections.push_back(btReducedVector(dof, indices, vecs3)); m_projections.push_back(btReducedVector(dof, indices, vecs3));
} }
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{ {
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
btAlignedObjectArray<int> indices; btAlignedObjectArray<int> indices;
btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
indices.push_back(index); indices.push_back(index);
vecs1.push_back(btVector3(1,0,0)); vecs1.push_back(btVector3(1, 0, 0));
vecs2.push_back(btVector3(0,1,0)); vecs2.push_back(btVector3(0, 1, 0));
vecs3.push_back(btVector3(0,0,1)); vecs3.push_back(btVector3(0, 0, 1));
m_projections.push_back(btReducedVector(dof, indices, vecs1)); m_projections.push_back(btReducedVector(dof, indices, vecs1));
m_projections.push_back(btReducedVector(dof, indices, vecs2)); m_projections.push_back(btReducedVector(dof, indices, vecs2));
m_projections.push_back(btReducedVector(dof, indices, vecs3)); m_projections.push_back(btReducedVector(dof, indices, vecs3));
} }
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{ {
int index = m_nodeRigidConstraints[i][j].m_node->index; 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; m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
btAlignedObjectArray<int> indices; btAlignedObjectArray<int> indices;
indices.push_back(index); indices.push_back(index);
btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
if (m_nodeRigidConstraints[i][j].m_static) if (m_nodeRigidConstraints[i][j].m_static)
{ {
vecs1.push_back(btVector3(1,0,0)); vecs1.push_back(btVector3(1, 0, 0));
vecs2.push_back(btVector3(0,1,0)); vecs2.push_back(btVector3(0, 1, 0));
vecs3.push_back(btVector3(0,0,1)); vecs3.push_back(btVector3(0, 0, 1));
m_projections.push_back(btReducedVector(dof, indices, vecs1)); m_projections.push_back(btReducedVector(dof, indices, vecs1));
m_projections.push_back(btReducedVector(dof, indices, vecs2)); m_projections.push_back(btReducedVector(dof, indices, vecs2));
m_projections.push_back(btReducedVector(dof, indices, vecs3)); m_projections.push_back(btReducedVector(dof, indices, vecs3));
} }
else else
{ {
vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
m_projections.push_back(btReducedVector(dof, indices, vecs1)); m_projections.push_back(btReducedVector(dof, indices, vecs1));
} }
} }
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{ {
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
} }
if (m_faceRigidConstraints[i][j].m_static) if (m_faceRigidConstraints[i][j].m_static)
{ {
for (int l = 0; l < 3; ++l) for (int l = 0; l < 3; ++l)
{ {
btReducedVector rv(dof); btReducedVector rv(dof);
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
rv.m_indices.push_back(face->m_n[k]->index); rv.m_indices.push_back(face->m_n[k]->index);
btVector3 v(0,0,0); btVector3 v(0, 0, 0);
v[l] = bary[k]; v[l] = bary[k];
rv.m_vecs.push_back(v); rv.m_vecs.push_back(v);
rv.sort(); rv.sort();
} }
m_projections.push_back(rv); m_projections.push_back(rv);
} }
@ -429,128 +428,128 @@ void btDeformableContactProjection::setProjection()
{ {
rv.m_indices.push_back(face->m_n[k]->index); rv.m_indices.push_back(face->m_n[k]->index);
rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
rv.sort(); rv.sort();
} }
m_projections.push_back(rv); m_projections.push_back(rv);
} }
} }
} }
btModifiedGramSchmidt<btReducedVector> mgs(m_projections); btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
mgs.solve(); mgs.solve();
m_projections = mgs.m_out; m_projections = mgs.m_out;
#endif #endif
} }
void btDeformableContactProjection::checkConstraints(const TVStack& x) void btDeformableContactProjection::checkConstraints(const TVStack& x)
{ {
for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
{ {
btVector3 d(0,0,0); btVector3 d(0, 0, 0);
const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
for (int j = 0; j < lm.m_num_constraints; ++j) for (int j = 0; j < lm.m_num_constraints; ++j)
{ {
for (int k = 0; k < lm.m_num_nodes; ++k) for (int k = 0; k < lm.m_num_nodes; ++k)
{ {
d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
} }
} }
printf("d = %f, %f, %f\n",d[0],d[1],d[2]); printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
} }
} }
void btDeformableContactProjection::setLagrangeMultiplier() void btDeformableContactProjection::setLagrangeMultiplier()
{ {
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < m_staticConstraints[i].size(); ++j) for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{ {
int index = m_staticConstraints[i][j].m_node->index; int index = m_staticConstraints[i][j].m_node->index;
m_staticConstraints[i][j].m_node->m_constrained = true; m_staticConstraints[i][j].m_node->m_constrained = true;
LagrangeMultiplier lm; LagrangeMultiplier lm;
lm.m_num_nodes = 1; lm.m_num_nodes = 1;
lm.m_indices[0] = index; lm.m_indices[0] = index;
lm.m_weights[0] = 1.0; lm.m_weights[0] = 1.0;
lm.m_num_constraints = 3; lm.m_num_constraints = 3;
lm.m_dirs[0] = btVector3(1,0,0); lm.m_dirs[0] = btVector3(1, 0, 0);
lm.m_dirs[1] = btVector3(0,1,0); lm.m_dirs[1] = btVector3(0, 1, 0);
lm.m_dirs[2] = btVector3(0,0,1); lm.m_dirs[2] = btVector3(0, 0, 1);
m_lagrangeMultipliers.push_back(lm); m_lagrangeMultipliers.push_back(lm);
} }
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{ {
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
LagrangeMultiplier lm; LagrangeMultiplier lm;
lm.m_num_nodes = 1; lm.m_num_nodes = 1;
lm.m_indices[0] = index; lm.m_indices[0] = index;
lm.m_weights[0] = 1.0; lm.m_weights[0] = 1.0;
lm.m_num_constraints = 3; lm.m_num_constraints = 3;
lm.m_dirs[0] = btVector3(1,0,0); lm.m_dirs[0] = btVector3(1, 0, 0);
lm.m_dirs[1] = btVector3(0,1,0); lm.m_dirs[1] = btVector3(0, 1, 0);
lm.m_dirs[2] = btVector3(0,0,1); lm.m_dirs[2] = btVector3(0, 0, 1);
m_lagrangeMultipliers.push_back(lm); m_lagrangeMultipliers.push_back(lm);
} }
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{ {
if (!m_nodeRigidConstraints[i][j].m_binding) if (!m_nodeRigidConstraints[i][j].m_binding)
{ {
continue; continue;
} }
int index = m_nodeRigidConstraints[i][j].m_node->index; int index = m_nodeRigidConstraints[i][j].m_node->index;
m_nodeRigidConstraints[i][j].m_node->m_constrained = true; m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
LagrangeMultiplier lm; LagrangeMultiplier lm;
lm.m_num_nodes = 1; lm.m_num_nodes = 1;
lm.m_indices[0] = index; lm.m_indices[0] = index;
lm.m_weights[0] = 1.0; lm.m_weights[0] = 1.0;
if (m_nodeRigidConstraints[i][j].m_static) if (m_nodeRigidConstraints[i][j].m_static)
{ {
lm.m_num_constraints = 3; lm.m_num_constraints = 3;
lm.m_dirs[0] = btVector3(1,0,0); lm.m_dirs[0] = btVector3(1, 0, 0);
lm.m_dirs[1] = btVector3(0,1,0); lm.m_dirs[1] = btVector3(0, 1, 0);
lm.m_dirs[2] = btVector3(0,0,1); lm.m_dirs[2] = btVector3(0, 0, 1);
} }
else else
{ {
lm.m_num_constraints = 1; lm.m_num_constraints = 1;
lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
} }
m_lagrangeMultipliers.push_back(lm); m_lagrangeMultipliers.push_back(lm);
} }
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{ {
if (!m_faceRigidConstraints[i][j].m_binding) if (!m_faceRigidConstraints[i][j].m_binding)
{ {
continue; continue;
} }
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
LagrangeMultiplier lm; LagrangeMultiplier lm;
lm.m_num_nodes = 3; lm.m_num_nodes = 3;
for (int k = 0; k<3; ++k) for (int k = 0; k < 3; ++k)
{ {
face->m_n[k]->m_constrained = true; face->m_n[k]->m_constrained = true;
lm.m_indices[k] = face->m_n[k]->index; lm.m_indices[k] = face->m_n[k]->index;
lm.m_weights[k] = bary[k]; lm.m_weights[k] = bary[k];
} }
if (m_faceRigidConstraints[i][j].m_static) if (m_faceRigidConstraints[i][j].m_static)
{ {
lm.m_num_constraints = 3; lm.m_num_constraints = 3;
lm.m_dirs[0] = btVector3(1,0,0); lm.m_dirs[0] = btVector3(1, 0, 0);
lm.m_dirs[1] = btVector3(0,1,0); lm.m_dirs[1] = btVector3(0, 1, 0);
lm.m_dirs[2] = btVector3(0,0,1); lm.m_dirs[2] = btVector3(0, 0, 1);
} }
else else
{ {
lm.m_num_constraints = 1; lm.m_num_constraints = 1;
lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
} }
m_lagrangeMultipliers.push_back(lm); m_lagrangeMultipliers.push_back(lm);
} }
} }
} }
@ -567,7 +566,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
if (node->m_im != 0) if (node->m_im != 0)
{ {
int index = node->index; int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im); f[index] += constraint.getDv(node) * (1. / node->m_im);
} }
} }
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
@ -580,7 +579,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
if (node->m_im != 0) if (node->m_im != 0)
{ {
int index = node->index; int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im); f[index] += constraint.getDv(node) * (1. / node->m_im);
} }
} }
} }
@ -592,7 +591,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
if (node->m_im != 0) if (node->m_im != 0)
{ {
int index = node->index; int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im); f[index] += constraint.getDv(node) * (1. / node->m_im);
} }
for (int k = 0; k < 3; ++k) for (int k = 0; k < 3; ++k)
{ {
@ -600,7 +599,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
if (node->m_im != 0) if (node->m_im != 0)
{ {
int index = node->index; int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im); f[index] += constraint.getDv(node) * (1. / node->m_im);
} }
} }
} }
@ -617,9 +616,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
m_nodeRigidConstraints.resize(N); m_nodeRigidConstraints.resize(N);
m_faceRigidConstraints.resize(N); m_faceRigidConstraints.resize(N);
m_deformableConstraints.resize(N); m_deformableConstraints.resize(N);
} }
for (int i = 0 ; i < N; ++i) for (int i = 0; i < N; ++i)
{ {
m_staticConstraints[i].clear(); m_staticConstraints[i].clear();
m_nodeAnchorConstraints[i].clear(); m_nodeAnchorConstraints[i].clear();
@ -628,12 +626,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
m_deformableConstraints[i].clear(); m_deformableConstraints[i].clear();
} }
#ifndef USE_MGS #ifndef USE_MGS
m_projectionsDict.clear(); m_projectionsDict.clear();
#else #else
m_projections.clear(); m_projections.clear();
#endif #endif
m_lagrangeMultipliers.clear(); m_lagrangeMultipliers.clear();
} }

101
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -27,31 +27,30 @@
struct LagrangeMultiplier struct LagrangeMultiplier
{ {
int m_num_constraints; // Number of constraints int m_num_constraints; // Number of constraints
int m_num_nodes; // Number of nodes in these constraints int m_num_nodes; // Number of nodes in these constraints
btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes
btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints;
int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes;
}; };
class btDeformableContactProjection class btDeformableContactProjection
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies; btAlignedObjectArray<btSoftBody*>& m_softBodies;
// all constraints involving face // all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
#ifndef USE_MGS #ifndef USE_MGS
// map from node index to projection directions // map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
#else #else
btAlignedObjectArray<btReducedVector> m_projections; btAlignedObjectArray<btReducedVector> m_projections;
#endif #endif
btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
// map from node index to static constraint // map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints; btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
// map from node index to node rigid constraint // map from node index to node rigid constraint
@ -62,39 +61,39 @@ public:
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints; btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
// map from node index to node anchor constraint // map from node index to node anchor constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints; btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
bool m_useStrainLimiting; bool m_useStrainLimiting;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies) btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
: m_softBodies(softBodies) : m_softBodies(softBodies)
{ {
} }
virtual ~btDeformableContactProjection() virtual ~btDeformableContactProjection()
{ {
} }
// apply the constraints to the rhs of the linear solve // apply the constraints to the rhs of the linear solve
virtual void project(TVStack& x); virtual void project(TVStack& x);
// add friction force to the rhs of the linear solve // add friction force to the rhs of the linear solve
virtual void applyDynamicFriction(TVStack& f); virtual void applyDynamicFriction(TVStack& f);
// update and solve the constraints // update and solve the constraints
virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
virtual void setConstraints(const btContactSolverInfo& infoGlobal); virtual void setConstraints(const btContactSolverInfo& infoGlobal);
// Set up projections for each vertex by adding the projection direction to // Set up projections for each vertex by adding the projection direction to
virtual void setProjection(); virtual void setProjection();
virtual void reinitialize(bool nodeUpdated); virtual void reinitialize(bool nodeUpdated);
btScalar solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
virtual void setLagrangeMultiplier(); virtual void setLagrangeMultiplier();
void checkConstraints(const TVStack& x); void checkConstraints(const TVStack& x);
}; };
#endif /* btDeformableContactProjection_h */ #endif /* btDeformableContactProjection_h */

189
src/BulletSoftBody/btDeformableCorotatedForce.h
View File

@ -21,107 +21,104 @@
static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
{ {
static const btPolarDecomposition polar; static const btPolarDecomposition polar;
return polar.decompose(m, q, s); return polar.decompose(m, q, s);
} }
class btDeformableCorotatedForce : public btDeformableLagrangianForce class btDeformableCorotatedForce : public btDeformableLagrangianForce
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda; btScalar m_mu, m_lambda;
btDeformableCorotatedForce(): m_mu(1), m_lambda(1) btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
{ {
}
}
btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) {
{ }
}
virtual void addScaledForces(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force) {
{ addScaledElasticForce(scale, force);
addScaledElasticForce(scale, force); }
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) {
{ addScaledElasticForce(scale, force);
addScaledElasticForce(scale, force); }
}
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
virtual void addScaledDampingForce(btScalar scale, TVStack& force) {
{ }
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
virtual void addScaledElasticForce(btScalar scale, TVStack& force) {
{ int numNodes = getNumNodes();
int numNodes = getNumNodes(); btAssert(numNodes <= force.size());
btAssert(numNodes <= force.size()); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); for (int i = 0; i < m_softBodies.size(); ++i)
for (int i = 0; i < m_softBodies.size(); ++i) {
{ btSoftBody* psb = m_softBodies[i];
btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_tetras.size(); ++j)
for (int j = 0; j < psb->m_tetras.size(); ++j) {
{ btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btMatrix3x3 P;
btMatrix3x3 P; firstPiola(tetra.m_F, P);
firstPiola(tetra.m_F,P); btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
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();
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node1 = tetra.m_n[1]; btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node2 = tetra.m_n[2]; btSoftBody::Node* node3 = tetra.m_n[3];
btSoftBody::Node* node3 = tetra.m_n[3]; size_t id0 = node0->index;
size_t id0 = node0->index; size_t id1 = node1->index;
size_t id1 = node1->index; size_t id2 = node2->index;
size_t id2 = node2->index; size_t id3 = node3->index;
size_t id3 = node3->index;
// elastic force
// elastic force // explicit elastic force
// explicit elastic force btScalar scale1 = scale * tetra.m_element_measure;
btScalar scale1 = scale * tetra.m_element_measure; force[id0] -= scale1 * force_on_node0;
force[id0] -= scale1 * force_on_node0; force[id1] -= scale1 * force_on_node123.getColumn(0);
force[id1] -= scale1 * force_on_node123.getColumn(0); force[id2] -= scale1 * force_on_node123.getColumn(1);
force[id2] -= scale1 * force_on_node123.getColumn(1); force[id3] -= scale1 * force_on_node123.getColumn(2);
force[id3] -= scale1 * force_on_node123.getColumn(2); }
} }
} }
}
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) {
{ // btMatrix3x3 JFinvT = F.adjoint();
// btMatrix3x3 JFinvT = F.adjoint(); btScalar J = F.determinant();
btScalar J = F.determinant(); P = F.adjoint().transpose() * (m_lambda * (J - 1));
P = F.adjoint().transpose() * (m_lambda * (J-1)); if (m_mu > SIMD_EPSILON)
if (m_mu > SIMD_EPSILON) {
{ btMatrix3x3 R, S;
btMatrix3x3 R,S; if (J < 1024 * SIMD_EPSILON)
if (J < 1024 * SIMD_EPSILON) R.setIdentity();
R.setIdentity(); else
else PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd /*https://fuchuyuan.github.io/research/svd/paper.pdf*/
/*https://fuchuyuan.github.io/research/svd/paper.pdf*/ P += (F - R) * 2 * m_mu;
P += (F-R) * 2 * m_mu; }
} }
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) {
{ }
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) {
{ }
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
virtual btDeformableLagrangianForceType getForceType()
virtual btDeformableLagrangianForceType getForceType() {
{ return BT_COROTATED_FORCE;
return BT_COROTATED_FORCE; }
}
}; };
#endif /* btCorotated_h */ #endif /* btCorotated_h */

160
src/BulletSoftBody/btDeformableGravityForce.h
View File

@ -21,87 +21,85 @@
class btDeformableGravityForce : public btDeformableLagrangianForce class btDeformableGravityForce : public btDeformableLagrangianForce
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btVector3 m_gravity; btVector3 m_gravity;
btDeformableGravityForce(const btVector3& g) : m_gravity(g)
{
}
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledGravityForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledGravityForce(scale, force);
}
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
virtual void addScaledGravityForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
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];
size_t id = n.index;
btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor;
force[id] += scaled_force;
}
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_GRAVITY_FORCE;
}
// the gravitational potential energy btDeformableGravityForce(const btVector3& g) : m_gravity(g)
virtual double totalEnergy(btScalar dt) {
{ }
double e = 0;
for (int i = 0; i<m_softBodies.size();++i) virtual void addScaledForces(btScalar scale, TVStack& force)
{ {
btSoftBody* psb = m_softBodies[i]; addScaledGravityForce(scale, force);
if (!psb->isActive()) }
{
continue; virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
} {
for (int j = 0; j < psb->m_nodes.size(); ++j) addScaledGravityForce(scale, force);
{ }
const btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0) virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{ {
e -= m_gravity.dot(node.m_q)/node.m_im; }
}
} virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
} {
return e; }
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
virtual void addScaledGravityForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
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];
size_t id = n.index;
btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor;
force[id] += scaled_force;
}
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_GRAVITY_FORCE;
}
// the gravitational potential energy
virtual double totalEnergy(btScalar dt)
{
double e = 0;
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];
if (node.m_im > 0)
{
e -= m_gravity.dot(node.m_q) / node.m_im;
}
}
}
return e;
}
}; };
#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

669
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@ -22,352 +22,349 @@
enum btDeformableLagrangianForceType enum btDeformableLagrangianForceType
{ {
BT_GRAVITY_FORCE = 1, BT_GRAVITY_FORCE = 1,
BT_MASSSPRING_FORCE = 2, BT_MASSSPRING_FORCE = 2,
BT_COROTATED_FORCE = 3, BT_COROTATED_FORCE = 3,
BT_NEOHOOKEAN_FORCE = 4, BT_NEOHOOKEAN_FORCE = 4,
BT_LINEAR_ELASTICITY_FORCE = 5, BT_LINEAR_ELASTICITY_FORCE = 5,
BT_MOUSE_PICKING_FORCE = 6 BT_MOUSE_PICKING_FORCE = 6
}; };
static inline double randomDouble(double low, double high) static inline double randomDouble(double low, double high)
{ {
return low + static_cast<double>(rand()) / RAND_MAX * (high - low); return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
} }
class btDeformableLagrangianForce class btDeformableLagrangianForce
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *> m_softBodies; btAlignedObjectArray<btSoftBody*> m_softBodies;
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
btDeformableLagrangianForce()
{
}
virtual ~btDeformableLagrangianForce(){}
// add all forces
virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
// add damping df
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
// build diagonal of A matrix
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
// add elastic df
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
// add all forces that are explicit in explicit solve
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
// add all damping forces
virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
virtual btDeformableLagrangianForceType getForceType() = 0;
virtual void reinitialize(bool nodeUpdated)
{
}
// get number of nodes that have the force
virtual int getNumNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
numNodes += m_softBodies[i]->m_nodes.size();
}
return numNodes;
}
// add a soft body to be affected by the particular lagrangian force
virtual void addSoftBody(btSoftBody* psb)
{
m_softBodies.push_back(psb);
}
virtual void removeSoftBody(btSoftBody* psb)
{
m_softBodies.remove(psb);
}
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
{
m_nodes = nodes;
}
// Calculate the incremental deformable generated from the input dx
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
{
btVector3 c1 = dx[id1] - dx[id0];
btVector3 c2 = dx[id2] - dx[id0];
btVector3 c3 = dx[id3] - dx[id0];
return btMatrix3x3(c1,c2,c3).transpose();
}
// Calculate the incremental deformable generated from the current velocity
virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
{
btVector3 c1 = n1->m_v - n0->m_v;
btVector3 c2 = n2->m_v - n0->m_v;
btVector3 c3 = n3->m_v - n0->m_v;
return btMatrix3x3(c1,c2,c3).transpose();
}
// test for addScaledElasticForce function
virtual void testDerivative()
{
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack dphi_dx;
dphi_dx.resize(dx.size());
for (int i =0; i < dphi_dx.size();++i)
{
dphi_dx[i].setZero();
}
addScaledForces(-1, dphi_dx);
// write down the current position
TVStack x;
x.resize(dx.size());
int 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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get dphi/dx * dx
double dphi = 0;
for (int i = 0; i < dx.size(); ++i)
{
dphi += dphi_dx[i].dot(dx[i]);
}
for (int i = 0; i<m_softBodies.size();++i) btDeformableLagrangianForce()
{ {
btSoftBody* psb = m_softBodies[i]; }
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f1 = totalElasticEnergy(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)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f2 = totalElasticEnergy(0);
//restore m_q
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = f1-f2-2*dphi;
errors.push_back(error);
std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
}
}
// test for addScaledElasticForce function
virtual void testHessian()
{
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack df;
df.resize(dx.size());
TVStack f1;
f1.resize(dx.size());
TVStack f2;
f2.resize(dx.size());
// write down the current position
TVStack x;
x.resize(dx.size());
int 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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get df
for (int i =0; i < df.size();++i)
{
df[i].setZero();
f1[i].setZero();
f2[i].setZero();
}
//set df virtual ~btDeformableLagrangianForce() {}
addScaledElasticForceDifferential(-1, dx, df);
// add all forces
for (int i = 0; i<m_softBodies.size();++i) virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
{
btSoftBody* psb = m_softBodies[i]; // add damping df
for (int j = 0; j < psb->m_nodes.size(); ++j) virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
{
psb->m_nodes[j].m_q = x[counter] + dx[counter]; // build diagonal of A matrix
counter++; virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
}
psb->updateDeformation(); // add elastic df
} virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
counter = 0;
// add all forces that are explicit in explicit solve
//set f1 virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
addScaledForces(-1, f1);
// add all damping forces
for (int i = 0; i<m_softBodies.size();++i) virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
{
btSoftBody* psb = m_softBodies[i]; virtual btDeformableLagrangianForceType getForceType() = 0;
for (int j = 0; j < psb->m_nodes.size(); ++j)
{ virtual void reinitialize(bool nodeUpdated)
psb->m_nodes[j].m_q = x[counter] - dx[counter]; {
counter++; }
}
psb->updateDeformation(); // get number of nodes that have the force
} virtual int getNumNodes()
counter = 0; {
int numNodes = 0;
//set f2 for (int i = 0; i < m_softBodies.size(); ++i)
addScaledForces(-1, f2); {
numNodes += m_softBodies[i]->m_nodes.size();
//restore m_q }
for (int i = 0; i<m_softBodies.size();++i) return numNodes;
{ }
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) // add a soft body to be affected by the particular lagrangian force
{ virtual void addSoftBody(btSoftBody* psb)
psb->m_nodes[j].m_q = x[counter]; {
counter++; m_softBodies.push_back(psb);
} }
psb->updateDeformation();
} virtual void removeSoftBody(btSoftBody* psb)
counter = 0; {
double error = 0; m_softBodies.remove(psb);
for (int i = 0; i < df.size();++i) }
{
btVector3 error_vector = f1[i]-f2[i]-2*df[i]; virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
error += error_vector.length2(); {
} m_nodes = nodes;
error = btSqrt(error); }
errors.push_back(error);
std::cout << "Iteration = " << it << ", error = " << error << std::endl; // Calculate the incremental deformable generated from the input dx
} virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
for (int i = 1; i < errors.size(); ++i) {
{ btVector3 c1 = dx[id1] - dx[id0];
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; btVector3 c2 = dx[id2] - dx[id0];
} btVector3 c3 = dx[id3] - dx[id0];
} return btMatrix3x3(c1, c2, c3).transpose();
}
//
virtual double totalElasticEnergy(btScalar dt) // Calculate the incremental deformable generated from the current velocity
{ virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
return 0; {
} btVector3 c1 = n1->m_v - n0->m_v;
btVector3 c2 = n2->m_v - n0->m_v;
// btVector3 c3 = n3->m_v - n0->m_v;
virtual double totalDampingEnergy(btScalar dt) return btMatrix3x3(c1, c2, c3).transpose();
{ }
return 0;
} // test for addScaledElasticForce function
virtual void testDerivative()
// total Energy takes dt as input because certain energies depend on dt {
virtual double totalEnergy(btScalar dt) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
return totalElasticEnergy(dt) + totalDampingEnergy(dt); btSoftBody* psb = m_softBodies[i];
} for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack dphi_dx;
dphi_dx.resize(dx.size());
for (int i = 0; i < dphi_dx.size(); ++i)
{
dphi_dx[i].setZero();
}
addScaledForces(-1, dphi_dx);
// write down the current position
TVStack x;
x.resize(dx.size());
int 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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get dphi/dx * dx
double dphi = 0;
for (int i = 0; i < dx.size(); ++i)
{
dphi += dphi_dx[i].dot(dx[i]);
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f1 = totalElasticEnergy(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)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f2 = totalElasticEnergy(0);
//restore m_q
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = f1 - f2 - 2 * dphi;
errors.push_back(error);
std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
}
}
// test for addScaledElasticForce function
virtual void testHessian()
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack df;
df.resize(dx.size());
TVStack f1;
f1.resize(dx.size());
TVStack f2;
f2.resize(dx.size());
// write down the current position
TVStack x;
x.resize(dx.size());
int 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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get df
for (int i = 0; i < df.size(); ++i)
{
df[i].setZero();
f1[i].setZero();
f2[i].setZero();
}
//set df
addScaledElasticForceDifferential(-1, dx, df);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
//set f1
addScaledForces(-1, f1);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
//set f2
addScaledForces(-1, f2);
//restore m_q
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = 0;
for (int i = 0; i < df.size(); ++i)
{
btVector3 error_vector = f1[i] - f2[i] - 2 * df[i];
error += error_vector.length2();
}
error = btSqrt(error);
errors.push_back(error);
std::cout << "Iteration = " << it << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
}
}
//
virtual double totalElasticEnergy(btScalar dt)
{
return 0;
}
//
virtual double totalDampingEnergy(btScalar dt)
{
return 0;
}
// total Energy takes dt as input because certain energies depend on dt
virtual double totalEnergy(btScalar dt)
{
return totalElasticEnergy(dt) + totalDampingEnergy(dt);
}
}; };
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

625
src/BulletSoftBody/btDeformableLinearElasticityForce.h
View File

@ -21,320 +21,319 @@
class btDeformableLinearElasticityForce : public btDeformableLagrangianForce class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda; btScalar m_mu, m_lambda;
btScalar m_mu_damp, m_lambda_damp; btScalar m_mu_damp, m_lambda_damp;
btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1) btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1)
{ {
btScalar damping = 0.05; btScalar damping = 0.05;
m_mu_damp = damping * m_mu; m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda; m_lambda_damp = damping * m_lambda;
} }
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
{ {
m_mu_damp = damping * m_mu; m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda; m_lambda_damp = damping * m_lambda;
} }
virtual void addScaledForces(btScalar scale, TVStack& force) virtual void addScaledForces(btScalar scale, TVStack& force)
{ {
addScaledDampingForce(scale, force); addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force); addScaledElasticForce(scale, force);
} }
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{ {
addScaledElasticForce(scale, force); addScaledElasticForce(scale, force);
} }
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
virtual void addScaledDampingForce(btScalar scale, TVStack& force) virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{ {
if (m_mu_damp == 0 && m_lambda_damp == 0) if (m_mu_damp == 0 && m_lambda_damp == 0)
return; return;
int numNodes = getNumNodes(); int numNodes = getNumNodes();
btAssert(numNodes <= force.size()); btAssert(numNodes <= force.size());
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < psb->m_tetras.size(); ++j) for (int j = 0; j < psb->m_tetras.size(); ++j)
{ {
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1]; btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2]; btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3]; btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index; size_t id0 = node0->index;
size_t id1 = node1->index; size_t id1 = node1->index;
size_t id2 = node2->index; size_t id2 = node2->index;
size_t id3 = node3->index; size_t id3 = node3->index;
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
btMatrix3x3 I; btMatrix3x3 I;
I.setIdentity(); I.setIdentity();
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; 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); // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); 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(); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
// damping force differential // damping force differential
btScalar scale1 = scale * tetra.m_element_measure; btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * df_on_node0; force[id0] -= scale1 * df_on_node0;
force[id1] -= scale1 * df_on_node123.getColumn(0); force[id1] -= scale1 * df_on_node123.getColumn(0);
force[id2] -= scale1 * df_on_node123.getColumn(1); force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2); force[id3] -= scale1 * df_on_node123.getColumn(2);
} }
} }
} }
virtual double totalElasticEnergy(btScalar dt) virtual double totalElasticEnergy(btScalar dt)
{ {
double energy = 0; double energy = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < psb->m_tetraScratches.size(); ++j) for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
{ {
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
energy += tetra.m_element_measure * elasticEnergyDensity(s); energy += tetra.m_element_measure * elasticEnergyDensity(s);
} }
} }
return energy; return energy;
} }
// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
virtual double totalDampingEnergy(btScalar dt) virtual double totalDampingEnergy(btScalar dt)
{ {
double energy = 0; double energy = 0;
int sz = 0; int sz = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
sz = btMax(sz, psb->m_nodes[j].index); sz = btMax(sz, psb->m_nodes[j].index);
} }
} }
TVStack dampingForce; TVStack dampingForce;
dampingForce.resize(sz+1); dampingForce.resize(sz + 1);
for (int i = 0; i < dampingForce.size(); ++i) for (int i = 0; i < dampingForce.size(); ++i)
dampingForce[i].setZero(); dampingForce[i].setZero();
addScaledDampingForce(0.5, dampingForce); addScaledDampingForce(0.5, dampingForce);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
const btSoftBody::Node& node = psb->m_nodes[j]; const btSoftBody::Node& node = psb->m_nodes[j];
energy -= dampingForce[node.index].dot(node.m_v) / dt; energy -= dampingForce[node.index].dot(node.m_v) / dt;
} }
} }
return energy; return energy;
} }
double elasticEnergyDensity(const btSoftBody::TetraScratch& s) double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
{ {
double density = 0; double density = 0;
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
density += m_lambda * trace * trace * 0.5; density += m_lambda * trace * trace * 0.5;
return density; return density;
} }
virtual void addScaledElasticForce(btScalar scale, TVStack& force) virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{ {
int numNodes = getNumNodes(); int numNodes = getNumNodes();
btAssert(numNodes <= force.size()); btAssert(numNodes <= force.size());
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
btScalar max_p = psb->m_cfg.m_maxStress; btScalar max_p = psb->m_cfg.m_maxStress;
for (int j = 0; j < psb->m_tetras.size(); ++j) for (int j = 0; j < psb->m_tetras.size(); ++j)
{ {
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Tetra& tetra = psb->m_tetras[j];
btMatrix3x3 P; btMatrix3x3 P;
firstPiola(psb->m_tetraScratches[j],P); firstPiola(psb->m_tetraScratches[j], P);
#if USE_SVD #if USE_SVD
if (max_p > 0) if (max_p > 0)
{ {
// since we want to clamp the principal stress to max_p, we only need to // since we want to clamp the principal stress to max_p, we only need to
// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
if (trPTP > max_p * max_p) if (trPTP > max_p * max_p)
{ {
btMatrix3x3 U, V; btMatrix3x3 U, V;
btVector3 sigma; btVector3 sigma;
singularValueDecomposition(P, U, sigma, V); singularValueDecomposition(P, U, sigma, V);
sigma[0] = btMin(sigma[0], max_p); sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p); sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p); sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p); sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p); sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p); sigma[2] = btMax(sigma[2], -max_p);
btMatrix3x3 Sigma; btMatrix3x3 Sigma;
Sigma.setIdentity(); Sigma.setIdentity();
Sigma[0][0] = sigma[0]; Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1]; Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2]; Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose(); P = U * Sigma * V.transpose();
} }
} }
#endif #endif
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); // 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(); btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1]; btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2]; btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3]; btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index; size_t id0 = node0->index;
size_t id1 = node1->index; size_t id1 = node1->index;
size_t id2 = node2->index; size_t id2 = node2->index;
size_t id3 = node3->index; size_t id3 = node3->index;
// elastic force // elastic force
btScalar scale1 = scale * tetra.m_element_measure; btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * force_on_node0; force[id0] -= scale1 * force_on_node0;
force[id1] -= scale1 * force_on_node123.getColumn(0); force[id1] -= scale1 * force_on_node123.getColumn(0);
force[id2] -= scale1 * force_on_node123.getColumn(1); force[id2] -= scale1 * force_on_node123.getColumn(1);
force[id3] -= scale1 * force_on_node123.getColumn(2); force[id3] -= scale1 * force_on_node123.getColumn(2);
} }
} }
} }
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{ {
if (m_mu_damp == 0 && m_lambda_damp == 0) if (m_mu_damp == 0 && m_lambda_damp == 0)
return; return;
int numNodes = getNumNodes(); int numNodes = getNumNodes();
btAssert(numNodes <= df.size()); btAssert(numNodes <= df.size());
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < psb->m_tetras.size(); ++j) for (int j = 0; j < psb->m_tetras.size(); ++j)
{ {
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1]; btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2]; btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3]; btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index; size_t id0 = node0->index;
size_t id1 = node1->index; size_t id1 = node1->index;
size_t id2 = node2->index; size_t id2 = node2->index;
size_t id3 = node3->index; size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
btMatrix3x3 I; btMatrix3x3 I;
I.setIdentity(); I.setIdentity();
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; 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); // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); // 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(); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
// damping force differential // damping force differential
btScalar scale1 = scale * tetra.m_element_measure; btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0; df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0); df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1); df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2); df[id3] -= scale1 * df_on_node123.getColumn(2);
} }
} }
} }
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{ {
int numNodes = getNumNodes(); int numNodes = getNumNodes();
btAssert(numNodes <= df.size()); btAssert(numNodes <= df.size());
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < psb->m_tetras.size(); ++j) for (int j = 0; j < psb->m_tetras.size(); ++j)
{ {
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1]; btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2]; btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3]; btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index; size_t id0 = node0->index;
size_t id1 = node1->index; size_t id1 = node1->index;
size_t id2 = node2->index; size_t id2 = node2->index;
size_t id3 = node3->index; size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP; btMatrix3x3 dP;
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); // 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(); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
// elastic force differential // elastic force differential
btScalar scale1 = scale * tetra.m_element_measure; btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0; df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0); df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1); df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2); df[id3] -= scale1 * df_on_node123.getColumn(2);
} }
} }
} }
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
{ {
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
} }
// Let P be the first piola stress. // Let P be the first piola stress.
// This function calculates the dP = dP/dF * dF // This function calculates the dP = dP/dF * dF
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
{ {
btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
} }
// Let Q be the damping stress. // Let Q be the damping stress.
// This function calculates the dP = dQ/dF * dF // This function calculates the dP = dQ/dF * dF
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
{ {
btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace; dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace;
} }
virtual btDeformableLagrangianForceType getForceType() virtual btDeformableLagrangianForceType getForceType()
{ {
return BT_LINEAR_ELASTICITY_FORCE; return BT_LINEAR_ELASTICITY_FORCE;
} }
}; };
#endif /* BT_LINEAR_ELASTICITY_H */ #endif /* BT_LINEAR_ELASTICITY_H */

544
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@ -20,282 +20,282 @@
class btDeformableMassSpringForce : public btDeformableLagrangianForce class btDeformableMassSpringForce : public btDeformableLagrangianForce
{ {
// If true, the damping force will be in the direction of the spring // If true, the damping force will be in the direction of the spring
// If false, the damping force will be in the direction of the velocity // If false, the damping force will be in the direction of the velocity
bool m_momentum_conserving; bool m_momentum_conserving;
btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
{ {
} }
btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
{ {
if (m_bendingStiffness < btScalar(0)) if (m_bendingStiffness < btScalar(0))
{ {
m_bendingStiffness = m_elasticStiffness; m_bendingStiffness = m_elasticStiffness;
} }
} }
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
// damping force
btVector3 v_diff = (node2->m_v - node1->m_v);
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
}
}
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar r = link.m_rl;
size_t id1 = node1->index;
size_t id2 = node2->index;
// elastic force
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
// implicit damping force differential
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)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); virtual void addScaledForces(btScalar scale, TVStack& force)
if (m_momentum_conserving) {
{ addScaledDampingForce(scale, force);
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) addScaledElasticForce(scale, force);
{ }
btVector3 dir = (node2->m_x - node1->m_x).normalized();
local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
}
}
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
}
}
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
{
// implicit damping force differential
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)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
for (int d = 0; d < 3; ++d)
{
if (node1->m_im > 0)
diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
if (node2->m_im > 0)
diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
}
}
}
else
{
for (int d = 0; d < 3; ++d)
{
if (node1->m_im > 0)
diagA[id1][d] -= scaled_k_damp;
if (node2->m_im > 0)
diagA[id2][d] -= scaled_k_damp;
}
}
}
}
}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 0;
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar r = link.m_rl;
// elastic force virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
btVector3 dir = (node2->m_q - node1->m_q); {
energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r); addScaledElasticForce(scale, force);
} }
}
return energy;
}
virtual double totalDampingEnergy(btScalar dt)
{
double energy = 0;
int sz = 0;
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);
}
}
TVStack dampingForce;
dampingForce.resize(sz+1);
for (int i = 0; i < dampingForce.size(); ++i)
dampingForce[i].setZero();
addScaledDampingForce(0.5, dampingForce);
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];
energy -= dampingForce[node.index].dot(node.m_v) / dt;
}
}
return energy;
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
// implicit damping force differential
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
btScalar r = link.m_rl;
btVector3 dir = (node1->m_q - node2->m_q); virtual void addScaledDampingForce(btScalar scale, TVStack& force)
btScalar dir_norm = dir.norm(); {
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); int numNodes = getNumNodes();
btVector3 dx_diff = dx[id1] - dx[id2]; btAssert(numNodes <= force.size());
btVector3 scaled_df = btVector3(0,0,0); for (int i = 0; i < m_softBodies.size(); ++i)
btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); {
if (dir_norm > SIMD_EPSILON) const btSoftBody* psb = m_softBodies[i];
{ if (!psb->isActive())
scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; {
scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized; continue;
scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff; }
} for (int j = 0; j < psb->m_links.size(); ++j)
{
df[id1] += scaled_df; const btSoftBody::Link& link = psb->m_links[j];
df[id2] -= scaled_df; btSoftBody::Node* node1 = link.m_n[0];
} btSoftBody::Node* node2 = link.m_n[1];
} size_t id1 = node1->index;
} size_t id2 = node2->index;
virtual btDeformableLagrangianForceType getForceType() // damping force
{ btVector3 v_diff = (node2->m_v - node1->m_v);
return BT_MASSSPRING_FORCE; btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
} if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
}
}
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar r = link.m_rl;
size_t id1 = node1->index;
size_t id2 = node2->index;
// elastic force
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
// implicit damping force differential
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)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
}
}
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
}
}
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
{
// implicit damping force differential
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)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
for (int d = 0; d < 3; ++d)
{
if (node1->m_im > 0)
diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
if (node2->m_im > 0)
diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
}
}
}
else
{
for (int d = 0; d < 3; ++d)
{
if (node1->m_im > 0)
diagA[id1][d] -= scaled_k_damp;
if (node2->m_im > 0)
diagA[id2][d] -= scaled_k_damp;
}
}
}
}
}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 0;
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar r = link.m_rl;
// elastic force
btVector3 dir = (node2->m_q - node1->m_q);
energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
}
}
return energy;
}
virtual double totalDampingEnergy(btScalar dt)
{
double energy = 0;
int sz = 0;
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);
}
}
TVStack dampingForce;
dampingForce.resize(sz + 1);
for (int i = 0; i < dampingForce.size(); ++i)
dampingForce[i].setZero();
addScaledDampingForce(0.5, dampingForce);
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];
energy -= dampingForce[node.index].dot(node.m_v) / dt;
}
}
return energy;
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
// implicit damping force differential
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];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
btScalar r = link.m_rl;
btVector3 dir = (node1->m_q - node2->m_q);
btScalar dir_norm = dir.norm();
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
btVector3 dx_diff = dx[id1] - dx[id2];
btVector3 scaled_df = btVector3(0, 0, 0);
btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
if (dir_norm > SIMD_EPSILON)
{
scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff;
}
df[id1] += scaled_df;
df[id2] -= scaled_df;
}
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_MASSSPRING_FORCE;
}
}; };
#endif /* btMassSpring_h */ #endif /* btMassSpring_h */

238
src/BulletSoftBody/btDeformableMousePickingForce.h
View File

@ -20,126 +20,126 @@
class btDeformableMousePickingForce : public btDeformableLagrangianForce class btDeformableMousePickingForce : public btDeformableLagrangianForce
{ {
// If true, the damping force will be in the direction of the spring // If true, the damping force will be in the direction of the spring
// If false, the damping force will be in the direction of the velocity // If false, the damping force will be in the direction of the velocity
btScalar m_elasticStiffness, m_dampingStiffness; btScalar m_elasticStiffness, m_dampingStiffness;
const btSoftBody::Face& m_face; const btSoftBody::Face& m_face;
btVector3 m_mouse_pos; btVector3 m_mouse_pos;
btScalar m_maxForce; btScalar m_maxForce;
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
{ {
} }
virtual void addScaledForces(btScalar scale, TVStack& force) virtual void addScaledForces(btScalar scale, TVStack& force)
{ {
addScaledDampingForce(scale, force); addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force); addScaledElasticForce(scale, force);
} }
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{ {
addScaledElasticForce(scale, force); addScaledElasticForce(scale, force);
} }
virtual void addScaledDampingForce(btScalar scale, TVStack& force) virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{ {
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
btVector3 v_diff = m_face.m_n[i]->m_v; btVector3 v_diff = m_face.m_n[i]->m_v;
btVector3 scaled_force = scale * m_dampingStiffness * v_diff; btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
{ {
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
} }
force[m_face.m_n[i]->index] -= scaled_force; force[m_face.m_n[i]->index] -= scaled_force;
} }
} }
virtual void addScaledElasticForce(btScalar scale, TVStack& force) virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{ {
btScalar scaled_stiffness = scale * m_elasticStiffness; btScalar scaled_stiffness = scale * m_elasticStiffness;
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
btVector3 scaled_force = scaled_stiffness * dir; btVector3 scaled_force = scaled_stiffness * dir;
if (scaled_force.safeNorm() > m_maxForce) if (scaled_force.safeNorm() > m_maxForce)
{ {
scaled_force.safeNormalize(); scaled_force.safeNormalize();
scaled_force *= m_maxForce; scaled_force *= m_maxForce;
} }
force[m_face.m_n[i]->index] -= scaled_force; force[m_face.m_n[i]->index] -= scaled_force;
} }
} }
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{ {
btScalar scaled_k_damp = m_dampingStiffness * scale; btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
{ {
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
} }
df[m_face.m_n[i]->index] -= local_scaled_df; df[m_face.m_n[i]->index] -= local_scaled_df;
} }
} }
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
virtual double totalElasticEnergy(btScalar dt) virtual double totalElasticEnergy(btScalar dt)
{ {
double energy = 0; double energy = 0;
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
btVector3 scaled_force = m_elasticStiffness * dir; btVector3 scaled_force = m_elasticStiffness * dir;
if (scaled_force.safeNorm() > m_maxForce) if (scaled_force.safeNorm() > m_maxForce)
{ {
scaled_force.safeNormalize(); scaled_force.safeNormalize();
scaled_force *= m_maxForce; scaled_force *= m_maxForce;
} }
energy += 0.5 * scaled_force.dot(dir); energy += 0.5 * scaled_force.dot(dir);
} }
return energy; return energy;
} }
virtual double totalDampingEnergy(btScalar dt) virtual double totalDampingEnergy(btScalar dt)
{ {
double energy = 0; double energy = 0;
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
btVector3 v_diff = m_face.m_n[i]->m_v; btVector3 v_diff = m_face.m_n[i]->m_v;
btVector3 scaled_force = m_dampingStiffness * v_diff; btVector3 scaled_force = m_dampingStiffness * v_diff;
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
{ {
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
} }
energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
} }
return energy; return energy;
} }
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{ {
//TODO //TODO
} }
void setMousePos(const btVector3& p) void setMousePos(const btVector3& p)
{ {
m_mouse_pos = p; m_mouse_pos = p;
} }
virtual btDeformableLagrangianForceType getForceType() virtual btDeformableLagrangianForceType getForceType()
{ {
return BT_MOUSE_PICKING_FORCE; return BT_MOUSE_PICKING_FORCE;
} }
}; };
#endif /* btMassSpring_h */ #endif /* btMassSpring_h */

209
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -13,133 +13,132 @@
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btDeformableMultiBodyConstraintSolver.h" #include "btDeformableMultiBodyConstraintSolver.h"
#include <iostream> #include <iostream>
// override the iterations method to include deformable/multibody contact // override the iterations method to include deformable/multibody contact
btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{ {
{ {
///this is a special step to resolve penetrations (just for contacts) ///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, 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; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++) for (int iteration = 0; iteration < maxIterations; iteration++)
{ {
// rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
// solve rigid/rigid in solver body // solve rigid/rigid in solver body
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// solver body velocity -> rigid body velocity // solver body velocity -> rigid body velocity
solverBodyWriteBack(infoGlobal); solverBodyWriteBack(infoGlobal);
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
// update rigid body velocity in rigid/deformable contact // update rigid body velocity in rigid/deformable contact
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
// solver body velocity <- rigid body velocity // solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal); writeToSolverBody(bodies, numBodies, infoGlobal);
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{ {
#ifdef VERBOSE_RESIDUAL_PRINTF #ifdef VERBOSE_RESIDUAL_PRINTF
if (iteration >= (maxIterations - 1)) if (iteration >= (maxIterations - 1))
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif #endif
m_analyticsData.m_numSolverCalls++; m_analyticsData.m_numSolverCalls++;
m_analyticsData.m_numIterationsUsed = iteration+1; m_analyticsData.m_numIterationsUsed = iteration + 1;
m_analyticsData.m_islandId = -2; m_analyticsData.m_islandId = -2;
if (numBodies>0) if (numBodies > 0)
m_analyticsData.m_islandId = bodies[0]->getCompanionId(); m_analyticsData.m_islandId = bodies[0]->getCompanionId();
m_analyticsData.m_numBodies = numBodies; m_analyticsData.m_numBodies = numBodies;
m_analyticsData.m_numContactManifolds = numManifolds; m_analyticsData.m_numContactManifolds = numManifolds;
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
break; break;
} }
} }
} }
return 0.f; return 0.f;
} }
void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
{ {
m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
// inherited from MultiBodyConstraintSolver // inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// overriden // overriden
solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// inherited from MultiBodyConstraintSolver // inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlyFinish(bodies, numBodies, info); solveGroupCacheFriendlyFinish(bodies, numBodies, info);
m_tmpMultiBodyConstraints = 0; m_tmpMultiBodyConstraints = 0;
m_tmpNumMultiBodyConstraints = 0; m_tmpNumMultiBodyConstraints = 0;
} }
void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{ {
for (int i = 0; i < numBodies; i++) for (int i = 0; i < numBodies; i++)
{ {
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
btRigidBody* body = btRigidBody::upcast(bodies[i]); btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass()) if (body && body->getInvMass())
{ {
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
} }
} }
} }
void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
{ {
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
{ {
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
if (body) if (body)
{ {
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
} }
} }
} }
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, 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"); BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
int iteration; int iteration;
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
{ {
{ {
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{ {
btScalar leastSquaresResidual = 0.f; btScalar leastSquaresResidual = 0.f;
{ {
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j; int j;
for (j = 0; j < numPoolConstraints; j++) for (j = 0; j < numPoolConstraints; j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
} }
// solve the position correction between deformable and rigid/multibody // solve the position correction between deformable and rigid/multibody
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal); btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
} }
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{ {
#ifdef VERBOSE_RESIDUAL_PRINTF #ifdef VERBOSE_RESIDUAL_PRINTF
if (iteration >= (infoGlobal.m_numIterations - 1)) if (iteration >= (infoGlobal.m_numIterations - 1))
printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration); printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif #endif
break; break;
} }
} }
} }
} }
} }

46
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@ -13,7 +13,6 @@
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H #ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H #define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
@ -32,30 +31,31 @@ class btDeformableBodySolver;
ATTRIBUTE_ALIGNED16(class) ATTRIBUTE_ALIGNED16(class)
btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
{ {
btDeformableBodySolver* m_deformableSolver; btDeformableBodySolver* m_deformableSolver;
protected: protected:
// override the iterations method to include deformable/multibody contact // override the iterations method to include deformable/multibody contact
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
// write the velocity of the the solver body to the underlying rigid body // write the velocity of the the solver body to the underlying rigid body
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
// 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, 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);
// 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, 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: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
void setDeformableSolver(btDeformableBodySolver* deformableSolver) void setDeformableSolver(btDeformableBodySolver * deformableSolver)
{ {
m_deformableSolver = deformableSolver; m_deformableSolver = deformableSolver;
} }
virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
}; };
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

1058
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

File diff suppressed because it is too large Load Diff

496
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@ -36,185 +36,185 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
{ {
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
///Solver classes that encapsulate multiple deformable bodies for solving ///Solver classes that encapsulate multiple deformable bodies for solving
btDeformableBodySolver* m_deformableBodySolver; btDeformableBodySolver* m_deformableBodySolver;
btSoftBodyArray m_softBodies; btSoftBodyArray m_softBodies;
int m_drawFlags; int m_drawFlags;
bool m_drawNodeTree; bool m_drawNodeTree;
bool m_drawFaceTree; bool m_drawFaceTree;
bool m_drawClusterTree; bool m_drawClusterTree;
btSoftBodyWorldInfo m_sbi; btSoftBodyWorldInfo m_sbi;
btScalar m_internalTime; btScalar m_internalTime;
int m_ccdIterations; int m_ccdIterations;
bool m_implicit; bool m_implicit;
bool m_lineSearch; bool m_lineSearch;
bool m_useProjection; bool m_useProjection;
DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback; btSolverCallback m_solverCallback;
protected: protected:
virtual void internalSingleStepSimulation(btScalar timeStep); virtual void internalSingleStepSimulation(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep); virtual void integrateTransforms(btScalar timeStep);
void positionCorrection(btScalar timeStep); void positionCorrection(btScalar timeStep);
void solveConstraints(btScalar timeStep); void solveConstraints(btScalar timeStep);
void updateActivationState(btScalar timeStep); void updateActivationState(btScalar timeStep);
void clearGravity(); void clearGravity();
public: public:
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
virtual void debugDrawWorld(); virtual void debugDrawWorld();
void setSolverCallback(btSolverCallback cb) void setSolverCallback(btSolverCallback cb)
{ {
m_solverCallback = cb; m_solverCallback = cb;
} }
virtual ~btDeformableMultiBodyDynamicsWorld(); virtual ~btDeformableMultiBodyDynamicsWorld();
virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
{ {
return (btMultiBodyDynamicsWorld*)(this); return (btMultiBodyDynamicsWorld*)(this);
} }
virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
{ {
return (const btMultiBodyDynamicsWorld*)(this); return (const btMultiBodyDynamicsWorld*)(this);
} }
virtual btDynamicsWorldType getWorldType() const virtual btDynamicsWorldType getWorldType() const
{ {
return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
} }
virtual void predictUnconstraintMotion(btScalar timeStep); virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
btSoftBodyArray& getSoftBodyArray() btSoftBodyArray& getSoftBodyArray()
{ {
return m_softBodies; return m_softBodies;
} }
const btSoftBodyArray& getSoftBodyArray() const const btSoftBodyArray& getSoftBodyArray() const
{ {
return m_softBodies; return m_softBodies;
} }
btSoftBodyWorldInfo& getWorldInfo() btSoftBodyWorldInfo& getWorldInfo()
{ {
return m_sbi; return m_sbi;
} }
const btSoftBodyWorldInfo& getWorldInfo() const const btSoftBodyWorldInfo& getWorldInfo() const
{ {
return m_sbi; return m_sbi;
} }
void reinitialize(btScalar timeStep); void reinitialize(btScalar timeStep);
void applyRigidBodyGravity(btScalar timeStep); void applyRigidBodyGravity(btScalar timeStep);
void beforeSolverCallbacks(btScalar timeStep); void beforeSolverCallbacks(btScalar timeStep);
void afterSolverCallbacks(btScalar timeStep); void afterSolverCallbacks(btScalar timeStep);
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeSoftBody(btSoftBody* body); void removeSoftBody(btSoftBody* body);
void removeCollisionObject(btCollisionObject* collisionObject); void removeCollisionObject(btCollisionObject* collisionObject);
int getDrawFlags() const { return (m_drawFlags); } int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; } void setDrawFlags(int f) { m_drawFlags = f; }
void setupConstraints(); void setupConstraints();
void performDeformableCollisionDetection(); void performDeformableCollisionDetection();
void solveMultiBodyConstraints(); void solveMultiBodyConstraints();
void solveContactConstraints(); void solveContactConstraints();
void sortConstraints(); void sortConstraints();
void softBodySelfCollision(); void softBodySelfCollision();
void setImplicit(bool implicit) void setImplicit(bool implicit)
{ {
m_implicit = implicit; m_implicit = implicit;
} }
void setLineSearch(bool lineSearch) void setLineSearch(bool lineSearch)
{ {
m_lineSearch = lineSearch; m_lineSearch = lineSearch;
} }
void applyRepulsionForce(btScalar timeStep); void applyRepulsionForce(btScalar timeStep);
void performGeometricCollisions(btScalar timeStep); void performGeometricCollisions(btScalar timeStep);
struct btDeformableSingleRayCallback : public btBroadphaseRayCallback struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
{ {
btVector3 m_rayFromWorld; btVector3 m_rayFromWorld;
btVector3 m_rayToWorld; btVector3 m_rayToWorld;
btTransform m_rayFromTrans; btTransform m_rayFromTrans;
btTransform m_rayToTrans; btTransform m_rayToTrans;
btVector3 m_hitNormal; btVector3 m_hitNormal;
const btDeformableMultiBodyDynamicsWorld* m_world; const btDeformableMultiBodyDynamicsWorld* m_world;
btCollisionWorld::RayResultCallback& m_resultCallback; btCollisionWorld::RayResultCallback& m_resultCallback;
btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
: m_rayFromWorld(rayFromWorld), : m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld), m_rayToWorld(rayToWorld),
m_world(world), m_world(world),
m_resultCallback(resultCallback) m_resultCallback(resultCallback)
{ {
m_rayFromTrans.setIdentity(); m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin(m_rayFromWorld); m_rayFromTrans.setOrigin(m_rayFromWorld);
m_rayToTrans.setIdentity(); m_rayToTrans.setIdentity();
m_rayToTrans.setOrigin(m_rayToWorld); m_rayToTrans.setOrigin(m_rayToWorld);
btVector3 rayDir = (rayToWorld - rayFromWorld); btVector3 rayDir = (rayToWorld - rayFromWorld);
rayDir.normalize(); rayDir.normalize();
///what about division by zero? --> just set rayDirection[i] to INF/1e30 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
m_signs[0] = m_rayDirectionInverse[0] < 0.0; m_signs[0] = m_rayDirectionInverse[0] < 0.0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0; m_signs[1] = m_rayDirectionInverse[1] < 0.0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0; m_signs[2] = m_rayDirectionInverse[2] < 0.0;
m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
} }
virtual bool process(const btBroadphaseProxy* proxy) virtual bool process(const btBroadphaseProxy* proxy)
{ {
///terminate further ray tests, once the closestHitFraction reached zero ///terminate further ray tests, once the closestHitFraction reached zero
if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
return false; return false;
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
//only perform raycast if filterMask matches //only perform raycast if filterMask matches
if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
{ {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax; //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
#if 0 #if 0
#ifdef RECALCULATE_AABB #ifdef RECALCULATE_AABB
btVector3 collisionObjectAabbMin,collisionObjectAabbMax; btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
@ -225,87 +225,85 @@ public:
const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
#endif #endif
#endif #endif
//btScalar hitLambda = m_resultCallback.m_closestHitFraction; //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
//culling already done by broadphase //culling already done by broadphase
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
{ {
m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
collisionObject, collisionObject,
collisionObject->getCollisionShape(), collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(), collisionObject->getWorldTransform(),
m_resultCallback); m_resultCallback);
} }
} }
return true; return true;
} }
}; };
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
{
BT_PROFILE("rayTest");
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
{
BT_PROFILE("rayTest");
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
#ifndef USE_BRUTEFORCE_RAYBROADPHASE #ifndef USE_BRUTEFORCE_RAYBROADPHASE
m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
#else #else
for (int i = 0; i < this->getNumCollisionObjects(); i++) for (int i = 0; i < this->getNumCollisionObjects(); i++)
{ {
rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
} }
#endif //USE_BRUTEFORCE_RAYBROADPHASE #endif //USE_BRUTEFORCE_RAYBROADPHASE
} }
void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
btCollisionObject* collisionObject, btCollisionObject* collisionObject,
const btCollisionShape* collisionShape, const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform, const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback) const RayResultCallback& resultCallback) const
{ {
if (collisionShape->isSoftBody()) if (collisionShape->isSoftBody())
{ {
btSoftBody* softBody = btSoftBody::upcast(collisionObject); btSoftBody* softBody = btSoftBody::upcast(collisionObject);
if (softBody) if (softBody)
{ {
btSoftBody::sRayCast softResult; btSoftBody::sRayCast softResult;
if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
{ {
if (softResult.fraction <= resultCallback.m_closestHitFraction) if (softResult.fraction <= resultCallback.m_closestHitFraction)
{ {
btCollisionWorld::LocalShapeInfo shapeInfo; btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = 0; shapeInfo.m_shapePart = 0;
shapeInfo.m_triangleIndex = softResult.index; shapeInfo.m_triangleIndex = softResult.index;
// get the normal // get the normal
btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
btVector3 normal = -rayDir; btVector3 normal = -rayDir;
normal.normalize(); normal.normalize();
{ {
normal = softBody->m_faces[softResult.index].m_normal; normal = softBody->m_faces[softResult.index].m_normal;
if (normal.dot(rayDir) > 0) if (normal.dot(rayDir) > 0)
{ {
// normal always point toward origin of the ray // normal always point toward origin of the ray
normal = -normal; normal = -normal;
} }
} }
btCollisionWorld::LocalRayResult rayResult(collisionObject, btCollisionWorld::LocalRayResult rayResult(collisionObject,
&shapeInfo, &shapeInfo,
normal, normal,
softResult.fraction); softResult.fraction);
bool normalInWorldSpace = true; bool normalInWorldSpace = true;
resultCallback.addSingleResult(rayResult, normalInWorldSpace); resultCallback.addSingleResult(rayResult, normalInWorldSpace);
} }
} }
} }
} }
else else
{ {
btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
} }
} }
}; };
#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

703
src/BulletSoftBody/btDeformableNeoHookeanForce.h
View File

@ -23,30 +23,30 @@ subject to the following restrictions:
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda; // Lame Parameters btScalar m_mu, m_lambda; // Lame Parameters
btScalar m_E, m_nu; // Young's modulus and Poisson ratio btScalar m_E, m_nu; // Young's modulus and Poisson ratio
btScalar m_mu_damp, m_lambda_damp; btScalar m_mu_damp, m_lambda_damp;
btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
{ {
btScalar damping = 0.05; btScalar damping = 0.05;
m_mu_damp = damping * m_mu; m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda; m_lambda_damp = damping * m_lambda;
updateYoungsModulusAndPoissonRatio(); updateYoungsModulusAndPoissonRatio();
} }
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): 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_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda; m_lambda_damp = damping * m_lambda;
updateYoungsModulusAndPoissonRatio(); updateYoungsModulusAndPoissonRatio();
} }
void updateYoungsModulusAndPoissonRatio() void updateYoungsModulusAndPoissonRatio()
{ {
// conversion from Lame Parameters to Young's modulus and Poisson ratio // conversion from Lame Parameters to Young's modulus and Poisson ratio
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu); m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
m_nu = m_lambda * 0.5 / (m_mu + m_lambda); m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
} }
@ -55,21 +55,21 @@ public:
// conversion from Young's modulus and Poisson ratio to Lame Parameters // conversion from Young's modulus and Poisson ratio to Lame Parameters
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
m_mu = m_E * 0.5 / (1 + m_nu); m_mu = m_E * 0.5 / (1 + m_nu);
m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu)); m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
} }
void setYoungsModulus(btScalar E) void setYoungsModulus(btScalar E)
{ {
m_E = E; m_E = E;
updateLameParameters(); updateLameParameters();
} }
void setPoissonRatio(btScalar nu) void setPoissonRatio(btScalar nu)
{ {
m_nu = nu; m_nu = nu;
updateLameParameters(); updateLameParameters();
} }
void setDamping(btScalar damping) void setDamping(btScalar damping)
{ {
m_mu_damp = damping * m_mu; m_mu_damp = damping * m_mu;
@ -83,339 +83,338 @@ public:
updateYoungsModulusAndPoissonRatio(); updateYoungsModulusAndPoissonRatio();
} }
virtual void addScaledForces(btScalar scale, TVStack& force) virtual void addScaledForces(btScalar scale, TVStack& force)
{ {
addScaledDampingForce(scale, force); addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force); addScaledElasticForce(scale, force);
} }
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
if (m_mu_damp == 0 && m_lambda_damp == 0)
return;
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
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];
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
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();
// damping force differential virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
btScalar scale1 = scale * tetra.m_element_measure; {
force[id0] -= scale1 * df_on_node0; addScaledElasticForce(scale, force);
force[id1] -= scale1 * df_on_node123.getColumn(0); }
force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2); // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
} virtual void addScaledDampingForce(btScalar scale, TVStack& force)
} {
} if (m_mu_damp == 0 && m_lambda_damp == 0)
return;
virtual double totalElasticEnergy(btScalar dt) int numNodes = getNumNodes();
{ btAssert(numNodes <= force.size());
double energy = 0; btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
if (!psb->isActive()) if (!psb->isActive())
{ {
continue; continue;
} }
for (int j = 0; j < psb->m_tetraScratches.size(); ++j) for (int j = 0; j < psb->m_tetras.size(); ++j)
{ {
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; btSoftBody::Node* node0 = tetra.m_n[0];
energy += tetra.m_element_measure * elasticEnergyDensity(s); btSoftBody::Node* node1 = tetra.m_n[1];
} btSoftBody::Node* node2 = tetra.m_n[2];
} btSoftBody::Node* node3 = tetra.m_n[3];
return energy; size_t id0 = node0->index;
} size_t id1 = node1->index;
size_t id2 = node2->index;
// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search size_t id3 = node3->index;
virtual double totalDampingEnergy(btScalar dt) btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
{ btMatrix3x3 I;
double energy = 0; I.setIdentity();
int sz = 0; btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
for (int i = 0; i < m_softBodies.size(); ++i) // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
{ btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
btSoftBody* psb = m_softBodies[i]; btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
if (!psb->isActive())
{ // damping force differential
continue; btScalar scale1 = scale * tetra.m_element_measure;
} force[id0] -= scale1 * df_on_node0;
for (int j = 0; j < psb->m_nodes.size(); ++j) force[id1] -= scale1 * df_on_node123.getColumn(0);
{ force[id2] -= scale1 * df_on_node123.getColumn(1);
sz = btMax(sz, psb->m_nodes[j].index); force[id3] -= scale1 * df_on_node123.getColumn(2);
} }
} }
TVStack dampingForce; }
dampingForce.resize(sz+1);
for (int i = 0; i < dampingForce.size(); ++i) virtual double totalElasticEnergy(btScalar dt)
dampingForce[i].setZero(); {
addScaledDampingForce(0.5, dampingForce); double energy = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) if (!psb->isActive())
{ {
const btSoftBody::Node& node = psb->m_nodes[j]; continue;
energy -= dampingForce[node.index].dot(node.m_v) / dt; }
} for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
} {
return energy; btSoftBody::Tetra& tetra = psb->m_tetras[j];
} btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
energy += tetra.m_element_measure * elasticEnergyDensity(s);
double elasticEnergyDensity(const btSoftBody::TetraScratch& s) }
{ }
double density = 0; return energy;
density += m_mu * 0.5 * (s.m_trace - 3.); }
density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda);
density -= m_mu * 0.5 * log(s.m_trace+1); // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
return density; virtual double totalDampingEnergy(btScalar dt)
} {
double energy = 0;
virtual void addScaledElasticForce(btScalar scale, TVStack& force) int sz = 0;
{ for (int i = 0; i < m_softBodies.size(); ++i)
int numNodes = getNumNodes(); {
btAssert(numNodes <= force.size()); btSoftBody* psb = m_softBodies[i];
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); if (!psb->isActive())
for (int i = 0; i < m_softBodies.size(); ++i) {
{ continue;
btSoftBody* psb = m_softBodies[i]; }
if (!psb->isActive()) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
continue; sz = btMax(sz, psb->m_nodes[j].index);
} }
btScalar max_p = psb->m_cfg.m_maxStress; }
for (int j = 0; j < psb->m_tetras.size(); ++j) TVStack dampingForce;
{ dampingForce.resize(sz + 1);
btSoftBody::Tetra& tetra = psb->m_tetras[j]; for (int i = 0; i < dampingForce.size(); ++i)
btMatrix3x3 P; dampingForce[i].setZero();
firstPiola(psb->m_tetraScratches[j],P); addScaledDampingForce(0.5, dampingForce);
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];
energy -= dampingForce[node.index].dot(node.m_v) / dt;
}
}
return energy;
}
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
{
double density = 0;
density += m_mu * 0.5 * (s.m_trace - 3.);
density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda);
density -= m_mu * 0.5 * log(s.m_trace + 1);
return density;
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes <= force.size());
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
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);
#ifdef USE_SVD #ifdef USE_SVD
if (max_p > 0) if (max_p > 0)
{ {
// since we want to clamp the principal stress to max_p, we only need to // since we want to clamp the principal stress to max_p, we only need to
// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
if (trPTP > max_p * max_p) if (trPTP > max_p * max_p)
{ {
btMatrix3x3 U, V; btMatrix3x3 U, V;
btVector3 sigma; btVector3 sigma;
singularValueDecomposition(P, U, sigma, V); singularValueDecomposition(P, U, sigma, V);
sigma[0] = btMin(sigma[0], max_p); sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p); sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p); sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p); sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p); sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p); sigma[2] = btMax(sigma[2], -max_p);
btMatrix3x3 Sigma; btMatrix3x3 Sigma;
Sigma.setIdentity(); Sigma.setIdentity();
Sigma[0][0] = sigma[0]; Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1]; Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2]; Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose(); P = U * Sigma * V.transpose();
} }
} }
#endif #endif
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); // 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(); btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
// elastic force
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * force_on_node0;
force[id1] -= scale1 * force_on_node123.getColumn(0);
force[id2] -= scale1 * force_on_node123.getColumn(1);
force[id3] -= scale1 * force_on_node123.getColumn(2);
}
}
}
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
if (m_mu_damp == 0 && m_lambda_damp == 0)
return;
int numNodes = getNumNodes();
btAssert(numNodes <= df.size());
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
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];
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
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;
// damping force differential btSoftBody::Node* node0 = tetra.m_n[0];
btScalar scale1 = scale * tetra.m_element_measure; btSoftBody::Node* node1 = tetra.m_n[1];
df[id0] -= scale1 * df_on_node0; btSoftBody::Node* node2 = tetra.m_n[2];
df[id1] -= scale1 * df_on_node123.getColumn(0); btSoftBody::Node* node3 = tetra.m_n[3];
df[id2] -= scale1 * df_on_node123.getColumn(1); size_t id0 = node0->index;
df[id3] -= scale1 * df_on_node123.getColumn(2); size_t id1 = node1->index;
} size_t id2 = node2->index;
} size_t id3 = node3->index;
}
// elastic force
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * force_on_node0;
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) force[id1] -= scale1 * force_on_node123.getColumn(0);
{ force[id2] -= scale1 * force_on_node123.getColumn(1);
int numNodes = getNumNodes(); force[id3] -= scale1 * force_on_node123.getColumn(2);
btAssert(numNodes <= df.size()); }
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); }
for (int i = 0; i < m_softBodies.size(); ++i) }
{
btSoftBody* psb = m_softBodies[i]; // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
if (!psb->isActive()) virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{ {
continue; if (m_mu_damp == 0 && m_lambda_damp == 0)
} return;
for (int j = 0; j < psb->m_tetras.size(); ++j) int numNodes = getNumNodes();
{ btAssert(numNodes <= df.size());
btSoftBody::Tetra& tetra = psb->m_tetras[j]; btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
btSoftBody::Node* node0 = tetra.m_n[0]; for (int i = 0; i < m_softBodies.size(); ++i)
btSoftBody::Node* node1 = tetra.m_n[1]; {
btSoftBody::Node* node2 = tetra.m_n[2]; btSoftBody* psb = m_softBodies[i];
btSoftBody::Node* node3 = tetra.m_n[3]; if (!psb->isActive())
size_t id0 = node0->index; {
size_t id1 = node1->index; continue;
size_t id2 = node2->index; }
size_t id3 = node3->index; for (int j = 0; j < psb->m_tetras.size(); ++j)
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; {
btMatrix3x3 dP; btSoftBody::Tetra& tetra = psb->m_tetras[j];
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); btSoftBody::Node* node0 = tetra.m_n[0];
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btSoftBody::Node* node1 = tetra.m_n[1];
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); btSoftBody::Node* node2 = tetra.m_n[2];
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
// elastic force differential size_t id1 = node1->index;
btScalar scale1 = scale * tetra.m_element_measure; size_t id2 = node2->index;
df[id0] -= scale1 * df_on_node0; size_t id3 = node3->index;
df[id1] -= scale1 * df_on_node123.getColumn(0); btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
df[id2] -= scale1 * df_on_node123.getColumn(1); btMatrix3x3 I;
df[id3] -= scale1 * df_on_node123.getColumn(2); 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();
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
{
btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.))); // damping force differential
btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); btScalar scale1 = scale * tetra.m_element_measure;
P = s.m_F * c1 + s.m_cofF * c2; df[id0] -= scale1 * df_on_node0;
} df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
// Let P be the first piola stress. df[id3] -= scale1 * df_on_node123.getColumn(2);
// This function calculates the dP = dP/dF * dF }
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) }
{ }
btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
dP = dF * c1 + s.m_F * c2; virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); {
dP += s.m_cofF * c3; int numNodes = getNumNodes();
} btAssert(numNodes <= df.size());
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
// Let Q be the damping stress. for (int i = 0; i < m_softBodies.size(); ++i)
// This function calculates the dP = dQ/dF * dF {
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) btSoftBody* psb = m_softBodies[i];
{ if (!psb->isActive())
btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.))); {
btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace)))); continue;
btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); }
dP = dF * c1 + s.m_F * c2; for (int j = 0; j < psb->m_tetras.size(); ++j)
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); {
dP += s.m_cofF * c3; btSoftBody::Tetra& tetra = psb->m_tetras[j];
} btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) btSoftBody::Node* node2 = tetra.m_n[2];
{ btSoftBody::Node* node3 = tetra.m_n[3];
btScalar ans = 0; size_t id0 = node0->index;
for (int i = 0; i < 3; ++i) size_t id1 = node1->index;
{ size_t id2 = node2->index;
ans += A[i].dot(B[i]); size_t id3 = node3->index;
} btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
return ans; btMatrix3x3 dP;
} firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
// Let C(A) be the cofactor of the matrix A btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
// Let H = the derivative of C(A) with respect to A evaluated at F = A btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
// This function calculates H*dF
void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) // elastic force differential
{ btScalar scale1 = scale * tetra.m_element_measure;
M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); df[id0] -= scale1 * df_on_node0;
M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); df[id1] -= scale1 * df_on_node123.getColumn(0);
M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); df[id2] -= scale1 * df_on_node123.getColumn(1);
M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); df[id3] -= scale1 * df_on_node123.getColumn(2);
M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); }
M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); }
M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); }
M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
} {
btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
virtual btDeformableLagrangianForceType getForceType() btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
{ P = s.m_F * c1 + s.m_cofF * c2;
return BT_NEOHOOKEAN_FORCE; }
}
// Let P be the first piola stress.
// This function calculates the dP = dP/dF * dF
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
{
btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.));
btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)));
btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
dP = dF * c1 + s.m_F * c2;
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP);
dP += s.m_cofF * c3;
}
// Let Q be the damping stress.
// This function calculates the dP = dQ/dF * dF
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
{
btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.)));
btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))));
btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
dP = dF * c1 + s.m_F * c2;
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP);
dP += s.m_cofF * c3;
}
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
{
btScalar ans = 0;
for (int i = 0; i < 3; ++i)
{
ans += A[i].dot(B[i]);
}
return ans;
}
// Let C(A) be the cofactor of the matrix A
// Let H = the derivative of C(A) with respect to A evaluated at F = A
// This function calculates H*dF
void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
{
M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_NEOHOOKEAN_FORCE;
}
}; };
#endif /* BT_NEOHOOKEAN_H */ #endif /* BT_NEOHOOKEAN_H */

153
src/BulletSoftBody/btKrylovSolver.h
View File

@ -26,83 +26,82 @@
template <class MatrixX> template <class MatrixX>
class btKrylovSolver class btKrylovSolver
{ {
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
public: public:
int m_maxIterations; int m_maxIterations;
btScalar m_tolerance; btScalar m_tolerance;
btKrylovSolver(int maxIterations, btScalar tolerance) btKrylovSolver(int maxIterations, btScalar tolerance)
: m_maxIterations(maxIterations) : m_maxIterations(maxIterations), m_tolerance(tolerance)
, m_tolerance(tolerance) {
{ }
} virtual ~btKrylovSolver() {}
virtual ~btKrylovSolver(){} virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0;
virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0; virtual void reinitialize(const TVStack& b) = 0;
virtual void reinitialize(const TVStack& b) = 0; virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b)
{
virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b) // c = a-b
{ btAssert(a.size() == b.size());
// c = a-b TVStack c;
btAssert(a.size() == b.size()); c.resize(a.size());
TVStack c; for (int i = 0; i < a.size(); ++i)
c.resize(a.size()); {
for (int i = 0; i < a.size(); ++i) c[i] = a[i] - b[i];
{ }
c[i] = a[i] - b[i]; return c;
} }
return c;
} virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a)
{
virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a) return dot(a, a);
{ }
return dot(a,a);
} virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a)
{
virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a) btScalar ret = 0;
{ for (int i = 0; i < a.size(); ++i)
btScalar ret = 0; {
for (int i = 0; i < a.size(); ++i) for (int d = 0; d < 3; ++d)
{ {
for (int d = 0; d < 3; ++d) ret = btMax(ret, btFabs(a[i][d]));
{ }
ret = btMax(ret, btFabs(a[i][d])); }
} return ret;
} }
return ret;
} virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b)
{
virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b) btScalar ans(0);
{ for (int i = 0; i < a.size(); ++i)
btScalar ans(0); ans += a[i].dot(b[i]);
for (int i = 0; i < a.size(); ++i) return ans;
ans += a[i].dot(b[i]); }
return ans;
} virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
{
virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) // result += s*a
{ btAssert(a.size() == result.size());
// result += s*a for (int i = 0; i < a.size(); ++i)
btAssert(a.size() == result.size()); result[i] += s * a[i];
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)
{
virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) // result = a*s + b
{ TVStack result;
// result = a*s + b result.resize(a.size());
TVStack result; for (int i = 0; i < a.size(); ++i)
result.resize(a.size()); result[i] = s * a[i] + b[i];
for (int i = 0; i < a.size(); ++i) return result;
result[i] = s * a[i] + b[i]; }
return result;
} virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance)
{
virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance) m_tolerance = tolerance;
{ }
m_tolerance = tolerance;
}
}; };
#endif /* BT_KRYLOV_SOLVER_H */ #endif /* BT_KRYLOV_SOLVER_H */

471
src/BulletSoftBody/btPreconditioner.h
View File

@ -19,269 +19,266 @@
class Preconditioner class Preconditioner
{ {
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
virtual void operator()(const TVStack& x, TVStack& b) = 0; virtual void operator()(const TVStack& x, TVStack& b) = 0;
virtual void reinitialize(bool nodeUpdated) = 0; virtual void reinitialize(bool nodeUpdated) = 0;
virtual ~Preconditioner(){} virtual ~Preconditioner() {}
}; };
class DefaultPreconditioner : public Preconditioner class DefaultPreconditioner : public Preconditioner
{ {
public: public:
virtual void operator()(const TVStack& x, TVStack& b) virtual void operator()(const TVStack& x, TVStack& b)
{ {
btAssert(b.size() == x.size()); btAssert(b.size() == x.size());
for (int i = 0; i < b.size(); ++i) for (int i = 0; i < b.size(); ++i)
b[i] = x[i]; b[i] = x[i];
} }
virtual void reinitialize(bool nodeUpdated) virtual void reinitialize(bool nodeUpdated)
{ {
} }
virtual ~DefaultPreconditioner(){} virtual ~DefaultPreconditioner() {}
}; };
class MassPreconditioner : public Preconditioner class MassPreconditioner : public Preconditioner
{ {
btAlignedObjectArray<btScalar> m_inv_mass; btAlignedObjectArray<btScalar> m_inv_mass;
const btAlignedObjectArray<btSoftBody *>& m_softBodies; const btAlignedObjectArray<btSoftBody*>& m_softBodies;
public:
MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
{
}
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_inv_mass.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
m_inv_mass.push_back(psb->m_nodes[j].m_im);
}
}
}
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
btAssert(m_inv_mass.size() <= x.size());
for (int i = 0; i < m_inv_mass.size(); ++i)
{
b[i] = x[i] * m_inv_mass[i];
}
for (int i = m_inv_mass.size(); i < b.size(); ++i)
{
b[i] = x[i];
}
}
};
public:
MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
: m_softBodies(softBodies)
{
}
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_inv_mass.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
m_inv_mass.push_back(psb->m_nodes[j].m_im);
}
}
}
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
btAssert(m_inv_mass.size() <= x.size());
for (int i = 0; i < m_inv_mass.size(); ++i)
{
b[i] = x[i] * m_inv_mass[i];
}
for (int i = m_inv_mass.size(); i < b.size(); ++i)
{
b[i] = x[i];
}
}
};
class KKTPreconditioner : public Preconditioner class KKTPreconditioner : public Preconditioner
{ {
const btAlignedObjectArray<btSoftBody *>& m_softBodies; const btAlignedObjectArray<btSoftBody*>& m_softBodies;
const btDeformableContactProjection& m_projections; const btDeformableContactProjection& m_projections;
const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
TVStack m_inv_A, m_inv_S; TVStack m_inv_A, m_inv_S;
const btScalar& m_dt; const btScalar& m_dt;
const bool& m_implicit; const bool& m_implicit;
public: public:
KKTPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
: m_softBodies(softBodies) : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
, m_projections(projections) {
, m_lf(lf) }
, m_dt(dt)
, m_implicit(implicit) virtual void reinitialize(bool nodeUpdated)
{ {
} if (nodeUpdated)
{
virtual void reinitialize(bool nodeUpdated) int num_nodes = 0;
{ for (int i = 0; i < m_softBodies.size(); ++i)
if (nodeUpdated) {
{ btSoftBody* psb = m_softBodies[i];
int num_nodes = 0; num_nodes += psb->m_nodes.size();
for (int i = 0; i < m_softBodies.size(); ++i) }
{ m_inv_A.resize(num_nodes);
btSoftBody* psb = m_softBodies[i]; }
num_nodes += psb->m_nodes.size(); buildDiagonalA(m_inv_A);
} for (int i = 0; i < m_inv_A.size(); ++i)
m_inv_A.resize(num_nodes); {
} // printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
buildDiagonalA(m_inv_A); for (int d = 0; d < 3; ++d)
for (int i = 0; i < m_inv_A.size(); ++i) {
{ m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
// printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); }
for (int d = 0; d < 3; ++d) }
{ m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0/ m_inv_A[i][d]; // printf("S.size() = %d \n", m_inv_S.size());
} buildDiagonalS(m_inv_A, m_inv_S);
} for (int i = 0; i < m_inv_S.size(); ++i)
m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); {
// printf("S.size() = %d \n", m_inv_S.size()); // printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
buildDiagonalS(m_inv_A, m_inv_S); for (int d = 0; d < 3; ++d)
for (int i = 0; i < m_inv_S.size(); ++i) {
{ m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d];
// printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); }
for (int d = 0; d < 3; ++d) }
{ }
m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0/ m_inv_S[i][d];
} void buildDiagonalA(TVStack& diagA) const
} {
} size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
void buildDiagonalA(TVStack& diagA) const {
{ btSoftBody* psb = m_softBodies[i];
size_t counter = 0; for (int j = 0; j < psb->m_nodes.size(); ++j)
for (int i = 0; i < m_softBodies.size(); ++i) {
{ const btSoftBody::Node& node = psb->m_nodes[j];
btSoftBody* psb = m_softBodies[i]; diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
for (int j = 0; j < psb->m_nodes.size(); ++j) ++counter;
{ }
const btSoftBody::Node& node = psb->m_nodes[j]; }
diagA[counter] = (node.m_im == 0) ? btVector3(0,0,0) : btVector3(1.0/node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); if (m_implicit)
++counter; {
} printf("implicit not implemented\n");
} btAssert(false);
if (m_implicit) }
{ for (int i = 0; i < m_lf.size(); ++i)
printf("implicit not implemented\n"); {
btAssert(false); // add damping matrix
} m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
for (int i = 0; i < m_lf.size(); ++i) }
{ }
// add damping matrix
m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
} {
} for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) // S[k,k] = e_k^T * C A_d^-1 C^T * e_k
{ const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) btVector3& t = diagS[c];
{ t.setZero();
// S[k,k] = e_k^T * C A_d^-1 C^T * e_k for (int j = 0; j < lm.m_num_constraints; ++j)
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; {
btVector3& t = diagS[c]; for (int i = 0; i < lm.m_num_nodes; ++i)
t.setZero(); {
for (int j = 0; j < lm.m_num_constraints; ++j) for (int d = 0; d < 3; ++d)
{ {
for (int i = 0; i < lm.m_num_nodes; ++i) t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
{ }
for (int d = 0; d < 3; ++d) }
{ }
t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; }
} }
}
}
}
}
//#define USE_FULL_PRECONDITIONER //#define USE_FULL_PRECONDITIONER
#ifndef USE_FULL_PRECONDITIONER #ifndef USE_FULL_PRECONDITIONER
virtual void operator()(const TVStack& x, TVStack& b) virtual void operator()(const TVStack& x, TVStack& b)
{ {
btAssert(b.size() == x.size()); btAssert(b.size() == x.size());
for (int i = 0; i < m_inv_A.size(); ++i) for (int i = 0; i < m_inv_A.size(); ++i)
{ {
b[i] = x[i] * m_inv_A[i]; b[i] = x[i] * m_inv_A[i];
} }
int offset = m_inv_A.size(); int offset = m_inv_A.size();
for (int i = 0; i < m_inv_S.size(); ++i) for (int i = 0; i < m_inv_S.size(); ++i)
{ {
b[i+offset] = x[i+offset] * m_inv_S[i]; b[i + offset] = x[i + offset] * m_inv_S[i];
} }
} }
#else #else
virtual void operator()(const TVStack& x, TVStack& b) virtual void operator()(const TVStack& x, TVStack& b)
{ {
btAssert(b.size() == x.size()); btAssert(b.size() == x.size());
int offset = m_inv_A.size(); int offset = m_inv_A.size();
for (int i = 0; i < m_inv_A.size(); ++i) for (int i = 0; i < m_inv_A.size(); ++i)
{ {
b[i] = x[i] * m_inv_A[i]; b[i] = x[i] * m_inv_A[i];
} }
for (int i = 0; i < m_inv_S.size(); ++i) for (int i = 0; i < m_inv_S.size(); ++i)
{ {
b[i+offset].setZero(); b[i + offset].setZero();
} }
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{ {
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
// C * x // C * x
for (int d = 0; d < lm.m_num_constraints; ++d) for (int d = 0; d < lm.m_num_constraints; ++d)
{ {
for (int i = 0; i < lm.m_num_nodes; ++i) for (int i = 0; i < lm.m_num_nodes; ++i)
{ {
b[offset+c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
} }
} }
} }
for (int i = 0; i < m_inv_S.size(); ++i) for (int i = 0; i < m_inv_S.size(); ++i)
{ {
b[i+offset] = b[i+offset] * m_inv_S[i]; b[i + offset] = b[i + offset] * m_inv_S[i];
} }
for (int i = 0; i < m_inv_A.size(); ++i) for (int i = 0; i < m_inv_A.size(); ++i)
{ {
b[i].setZero(); b[i].setZero();
} }
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{ {
// C^T * lambda // C^T * lambda
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i) for (int i = 0; i < lm.m_num_nodes; ++i)
{ {
for (int j = 0; j < lm.m_num_constraints; ++j) for (int j = 0; j < lm.m_num_constraints; ++j)
{ {
b[lm.m_indices[i]] += b[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
} }
} }
} }
for (int i = 0; i < m_inv_A.size(); ++i) for (int i = 0; i < m_inv_A.size(); ++i)
{ {
b[i] = (x[i] - b[i]) * m_inv_A[i]; b[i] = (x[i] - b[i]) * m_inv_A[i];
} }
TVStack t; TVStack t;
t.resize(b.size()); t.resize(b.size());
for (int i = 0; i < m_inv_S.size(); ++i) for (int i = 0; i < m_inv_S.size(); ++i)
{ {
t[i+offset] = x[i+offset] * m_inv_S[i]; t[i + offset] = x[i + offset] * m_inv_S[i];
} }
for (int i = 0; i < m_inv_A.size(); ++i) for (int i = 0; i < m_inv_A.size(); ++i)
{ {
t[i].setZero(); t[i].setZero();
} }
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{ {
// C^T * lambda // C^T * lambda
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i) for (int i = 0; i < lm.m_num_nodes; ++i)
{ {
for (int j = 0; j < lm.m_num_constraints; ++j) for (int j = 0; j < lm.m_num_constraints; ++j)
{ {
t[lm.m_indices[i]] += t[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
} }
} }
} }
for (int i = 0; i < m_inv_A.size(); ++i) for (int i = 0; i < m_inv_A.size(); ++i)
{ {
b[i] += t[i] * m_inv_A[i]; b[i] += t[i] * m_inv_A[i];
} }
for (int i = 0; i < m_inv_S.size(); ++i) for (int i = 0; i < m_inv_S.size(); ++i)
{ {
b[i+offset] -= x[i+offset] * m_inv_S[i]; b[i + offset] -= x[i + offset] * m_inv_S[i];
} }
} }
#endif #endif
}; };

1247
src/BulletSoftBody/btSoftBody.cpp
View File

File diff suppressed because it is too large Load Diff

382
src/BulletSoftBody/btSoftBody.h
View File

@ -35,7 +35,7 @@ subject to the following restrictions:
//#else //#else
#define btSoftBodyData btSoftBodyFloatData #define btSoftBodyData btSoftBodyFloatData
#define btSoftBodyDataName "btSoftBodyFloatData" #define btSoftBodyDataName "btSoftBodyFloatData"
static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
static unsigned long seed = 243703; static unsigned long seed = 243703;
//#endif //BT_USE_DOUBLE_PRECISION //#endif //BT_USE_DOUBLE_PRECISION
@ -171,10 +171,10 @@ public:
CL_SELF = 0x0040, ///Cluster soft body self collision CL_SELF = 0x0040, ///Cluster soft body self collision
VF_DD = 0x0080, ///Vertex vs face soft vs soft handling VF_DD = 0x0080, ///Vertex vs face soft vs soft handling
RVDFmask = 0x0f00, /// Rigid versus deformable face mask RVDFmask = 0x0f00, /// Rigid versus deformable face mask
SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face
SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face
SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node
/* presets */ /* presets */
Default = SDF_RS, Default = SDF_RS,
END END
@ -226,7 +226,7 @@ public:
const btCollisionObject* m_colObj; /* Rigid body */ const btCollisionObject* m_colObj; /* Rigid body */
btVector3 m_normal; /* Outward normal */ btVector3 m_normal; /* Outward normal */
btScalar m_offset; /* Offset from origin */ btScalar m_offset; /* Offset from origin */
btVector3 m_bary; /* Barycentric weights for faces */ btVector3 m_bary; /* Barycentric weights for faces */
}; };
/* sMedium */ /* sMedium */
@ -263,7 +263,7 @@ public:
btVector3 m_x; // Position btVector3 m_x; // Position
btVector3 m_q; // Previous step position/Test position btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity btVector3 m_v; // Velocity
btVector3 m_vn; // Previous step velocity btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal btVector3 m_n; // Normal
btScalar m_im; // 1/mass btScalar m_im; // 1/mass
@ -291,37 +291,37 @@ public:
/* Face */ /* Face */
struct Face : Feature struct Face : Feature
{ {
Node* m_n[3]; // Node pointers Node* m_n[3]; // Node pointers
btVector3 m_normal; // Normal btVector3 m_normal; // Normal
btScalar m_ra; // Rest area btScalar m_ra; // Rest area
btDbvtNode* m_leaf; // Leaf data btDbvtNode* m_leaf; // Leaf data
btVector4 m_pcontact; // barycentric weights of the persistent contact btVector4 m_pcontact; // barycentric weights of the persistent contact
btVector3 m_n0, m_n1, m_vn; btVector3 m_n0, m_n1, m_vn;
int m_index; int m_index;
}; };
/* Tetra */ /* Tetra */
struct Tetra : Feature struct Tetra : Feature
{ {
Node* m_n[4]; // Node pointers Node* m_n[4]; // Node pointers
btScalar m_rv; // Rest volume btScalar m_rv; // Rest volume
btDbvtNode* m_leaf; // Leaf data btDbvtNode* m_leaf; // Leaf data
btVector3 m_c0[4]; // gradients btVector3 m_c0[4]; // gradients
btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
btScalar m_c2; // m_c1/sum(|g0..3|^2) btScalar m_c2; // m_c1/sum(|g0..3|^2)
btMatrix3x3 m_Dm_inverse; // rest Dm^-1 btMatrix3x3 m_Dm_inverse; // rest Dm^-1
btMatrix3x3 m_F; btMatrix3x3 m_F;
btScalar m_element_measure; btScalar m_element_measure;
}; };
/* TetraScratch */ /* TetraScratch */
struct TetraScratch struct TetraScratch
{ {
btMatrix3x3 m_F; // deformation gradient F btMatrix3x3 m_F; // deformation gradient F
btScalar m_trace; // trace of F^T * F btScalar m_trace; // trace of F^T * F
btScalar m_J; // det(F) btScalar m_J; // det(F)
btMatrix3x3 m_cofF; // cofactor of F btMatrix3x3 m_cofF; // cofactor of F
}; };
/* RContact */ /* RContact */
struct RContact struct RContact
{ {
@ -332,67 +332,67 @@ public:
btScalar m_c2; // ima*dt btScalar m_c2; // ima*dt
btScalar m_c3; // Friction btScalar m_c3; // Friction
btScalar m_c4; // Hardness btScalar m_c4; // Hardness
// jacobians and unit impulse responses for multibody // jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal; btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1; btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2; btMultiBodyJacobianData jacobianData_t2;
btVector3 t1; btVector3 t1;
btVector3 t2; btVector3 t2;
}; };
class DeformableRigidContact class DeformableRigidContact
{ {
public: public:
sCti m_cti; // Contact infos sCti m_cti; // Contact infos
btMatrix3x3 m_c0; // Impulse matrix btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor btVector3 m_c1; // Relative anchor
btScalar m_c2; // inverse mass of node/face btScalar m_c2; // inverse mass of node/face
btScalar m_c3; // Friction btScalar m_c3; // Friction
btScalar m_c4; // Hardness btScalar m_c4; // Hardness
// jacobians and unit impulse responses for multibody // jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal; btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1; btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2; btMultiBodyJacobianData jacobianData_t2;
btVector3 t1; btVector3 t1;
btVector3 t2; btVector3 t2;
}; };
class DeformableNodeRigidContact : public DeformableRigidContact class DeformableNodeRigidContact : public DeformableRigidContact
{ {
public: public:
Node* m_node; // Owner node Node* m_node; // Owner node
}; };
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
{ {
public: public:
btVector3 m_local; // Anchor position in body space btVector3 m_local; // Anchor position in body space
}; };
class DeformableFaceRigidContact : public DeformableRigidContact class DeformableFaceRigidContact : public DeformableRigidContact
{ {
public: public:
Face* m_face; // Owner face Face* m_face; // Owner face
btVector3 m_contactPoint; // Contact point btVector3 m_contactPoint; // Contact point
btVector3 m_bary; // Barycentric weights btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
}; };
struct DeformableFaceNodeContact struct DeformableFaceNodeContact
{ {
Node* m_node; // Node Node* m_node; // Node
Face* m_face; // Face Face* m_face; // Face
btVector3 m_bary; // Barycentric weights btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
btVector3 m_normal; // Normal btVector3 m_normal; // Normal
btScalar m_margin; // Margin btScalar m_margin; // Margin
btScalar m_friction; // Friction btScalar m_friction; // Friction
btScalar m_imf; // inverse mass of the face at contact point btScalar m_imf; // inverse mass of the face at contact point
btScalar m_c0; // scale of the impulse matrix; btScalar m_c0; // scale of the impulse matrix;
}; };
/* SContact */ /* SContact */
struct SContact struct SContact
{ {
@ -719,19 +719,19 @@ public:
tVSolverArray m_vsequence; // Velocity solvers sequence tVSolverArray m_vsequence; // Velocity solvers sequence
tPSolverArray m_psequence; // Position solvers sequence tPSolverArray m_psequence; // Position solvers sequence
tPSolverArray m_dsequence; // Drift solvers sequence tPSolverArray m_dsequence; // Drift solvers sequence
btScalar drag; // deformable air drag btScalar drag; // deformable air drag
btScalar m_maxStress; // Maximum principle first Piola stress btScalar m_maxStress; // Maximum principle first Piola stress
}; };
/* SolverState */ /* SolverState */
struct SolverState struct SolverState
{ {
//if you add new variables, always initialize them! //if you add new variables, always initialize them!
SolverState() SolverState()
:sdt(0), : sdt(0),
isdt(0), isdt(0),
velmrg(0), velmrg(0),
radmrg(0), radmrg(0),
updmrg(0) updmrg(0)
{ {
} }
btScalar sdt; // dt*timescale btScalar sdt; // dt*timescale
@ -792,41 +792,41 @@ public:
btSoftBodyWorldInfo* m_worldInfo; // World info btSoftBodyWorldInfo* m_worldInfo; // World info
tNoteArray m_notes; // Notes tNoteArray m_notes; // Notes
tNodeArray m_nodes; // Nodes tNodeArray m_nodes; // Nodes
tNodeArray m_renderNodes; // Nodes tNodeArray m_renderNodes; // Nodes
tLinkArray m_links; // Links tLinkArray m_links; // Links
tFaceArray m_faces; // Faces tFaceArray m_faces; // Faces
tFaceArray m_renderFaces; // Faces tFaceArray m_renderFaces; // Faces
tTetraArray m_tetras; // Tetras tTetraArray m_tetras; // Tetras
btAlignedObjectArray<TetraScratch> m_tetraScratches; btAlignedObjectArray<TetraScratch> m_tetraScratches;
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
tAnchorArray m_anchors; // Anchors tAnchorArray m_anchors; // Anchors
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
tRContactArray m_rcontacts; // Rigid contacts tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
tSContactArray m_scontacts; // Soft contacts tSContactArray m_scontacts; // Soft contacts
tJointArray m_joints; // Joints tJointArray m_joints; // Joints
tMaterialArray m_materials; // Materials tMaterialArray m_materials; // Materials
btScalar m_timeacc; // Time accumulator btScalar m_timeacc; // Time accumulator
btVector3 m_bounds[2]; // Spatial bounds btVector3 m_bounds[2]; // Spatial bounds
bool m_bUpdateRtCst; // Update runtime constants bool m_bUpdateRtCst; // Update runtime constants
btDbvt m_ndbvt; // Nodes tree btDbvt m_ndbvt; // Nodes tree
btDbvt m_fdbvt; // Faces tree btDbvt m_fdbvt; // Faces tree
btDbvntNode* m_fdbvnt; // Faces tree with normals btDbvntNode* m_fdbvnt; // Faces tree with normals
btDbvt m_cdbvt; // Clusters tree btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters tClusterArray m_clusters; // Clusters
btScalar m_dampingCoefficient; // Damping Coefficient btScalar m_dampingCoefficient; // Damping Coefficient
btScalar m_sleepingThreshold; btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared; btScalar m_maxSpeedSquared;
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btScalar m_repulsionStiffness; btScalar m_repulsionStiffness;
btScalar m_gravityFactor; btScalar m_gravityFactor;
btAlignedObjectArray<btVector3> m_X; // initial positions btAlignedObjectArray<btVector3> m_X; // initial positions
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation
bool m_useSelfCollision; bool m_useSelfCollision;
bool m_softSoftCollision; bool m_softSoftCollision;
@ -858,11 +858,11 @@ public:
{ {
return m_worldInfo; return m_worldInfo;
} }
void setDampingCoefficient(btScalar damping_coeff) void setDampingCoefficient(btScalar damping_coeff)
{ {
m_dampingCoefficient = damping_coeff; m_dampingCoefficient = damping_coeff;
} }
///@todo: avoid internal softbody shape hack and move collision code to collision library ///@todo: avoid internal softbody shape hack and move collision code to collision library
virtual void setCollisionShape(btCollisionShape* collisionShape) virtual void setCollisionShape(btCollisionShape* collisionShape)
@ -923,9 +923,9 @@ public:
Material* mat = 0); Material* mat = 0);
/* Append anchor */ /* Append anchor */
void appendDeformableAnchor(int node, btRigidBody* body); void appendDeformableAnchor(int node, btRigidBody* body);
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
void appendAnchor(int node, void appendAnchor(int node,
btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void removeAnchor(int node); void removeAnchor(int node);
@ -979,10 +979,10 @@ public:
void setLinearVelocity(const btVector3& linVel); void setLinearVelocity(const btVector3& linVel);
/* Set the angular velocity of the center of mass */ /* Set the angular velocity of the center of mass */
void setAngularVelocity(const btVector3& angVel); void setAngularVelocity(const btVector3& angVel);
/* Get best fit rigid transform */ /* Get best fit rigid transform */
btTransform getRigidTransform(); btTransform getRigidTransform();
/* Transform to given pose */ /* Transform to given pose */
void transformTo(const btTransform& trs); void transformTo(const btTransform& trs);
/* Transform */ /* Transform */
void transform(const btTransform& trs); void transform(const btTransform& trs);
/* Translate */ /* Translate */
@ -1071,11 +1071,11 @@ public:
/* defaultCollisionHandlers */ /* defaultCollisionHandlers */
void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
void defaultCollisionHandler(btSoftBody* psb); void defaultCollisionHandler(btSoftBody* psb);
void setSelfCollision(bool useSelfCollision); void setSelfCollision(bool useSelfCollision);
bool useSelfCollision(); bool useSelfCollision();
void updateDeactivation(btScalar timeStep); void updateDeactivation(btScalar timeStep);
void setZeroVelocity(); void setZeroVelocity();
bool wantsSleeping(); bool wantsSleeping();
// //
// Functionality to deal with new accelerated solvers. // Functionality to deal with new accelerated solvers.
@ -1154,8 +1154,8 @@ public:
void rebuildNodeTree(); void rebuildNodeTree();
btVector3 evaluateCom() const; btVector3 evaluateCom() const;
bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
void updateNormals(); void updateNormals();
void updateBounds(); void updateBounds();
void updatePose(); void updatePose();
@ -1169,15 +1169,15 @@ public:
void solveClusters(btScalar sor); void solveClusters(btScalar sor);
void applyClusters(bool drift); void applyClusters(bool drift);
void dampClusters(); void dampClusters();
void setSpringStiffness(btScalar k); void setSpringStiffness(btScalar k);
void setGravityFactor(btScalar gravFactor); void setGravityFactor(btScalar gravFactor);
void initializeDmInverse(); void initializeDmInverse();
void updateDeformation(); void updateDeformation();
void advanceDeformation(); void advanceDeformation();
void applyForces(); void applyForces();
void setMaxStress(btScalar maxStress); void setMaxStress(btScalar maxStress);
void interpolateRenderMesh(); void interpolateRenderMesh();
void setCollisionQuadrature(int N); void setCollisionQuadrature(int N);
static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);
@ -1186,14 +1186,15 @@ public:
static psolver_t getSolver(ePSolver::_ solver); static psolver_t getSolver(ePSolver::_ solver);
static vsolver_t getSolver(eVSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver);
void geometricCollisionHandler(btSoftBody* psb); void geometricCollisionHandler(btSoftBody* psb);
#define SAFE_EPSILON SIMD_EPSILON*100.0 #define SAFE_EPSILON SIMD_EPSILON * 100.0
void updateNode(btDbvtNode* node, bool use_velocity, bool margin) void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
{ {
if (node->isleaf()) if (node->isleaf())
{ {
btSoftBody::Node* n = (btSoftBody::Node*)(node->data); btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; ATTRIBUTE_ALIGNED16(btDbvtVolume)
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision vol;
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
if (use_velocity) if (use_velocity)
{ {
btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
@ -1211,38 +1212,40 @@ public:
{ {
updateNode(node->childs[0], use_velocity, margin); updateNode(node->childs[0], use_velocity, margin);
updateNode(node->childs[1], use_velocity, margin); updateNode(node->childs[1], use_velocity, margin);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
Merge(node->childs[0]->volume, node->childs[1]->volume, vol); Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
node->volume = vol; node->volume = vol;
} }
} }
void updateNodeTree(bool use_velocity, bool margin) void updateNodeTree(bool use_velocity, bool margin)
{ {
if (m_ndbvt.m_root) if (m_ndbvt.m_root)
updateNode(m_ndbvt.m_root, use_velocity, margin); updateNode(m_ndbvt.m_root, use_velocity, margin);
} }
template <class DBVTNODE> // btDbvtNode or btDbvntNode template <class DBVTNODE> // btDbvtNode or btDbvntNode
void updateFace(DBVTNODE* node, bool use_velocity, bool margin) void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
{ {
if (node->isleaf()) if (node->isleaf())
{ {
btSoftBody::Face* f = (btSoftBody::Face*)(node->data); btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
if (use_velocity) if (use_velocity)
{ {
btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v, btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
vol = btDbvtVolume::FromPoints(points, 6); vol = btDbvtVolume::FromPoints(points, 6);
} }
else else
{ {
btVector3 points[3] = {f->m_n[0]->m_x, btVector3 points[3] = {f->m_n[0]->m_x,
f->m_n[1]->m_x, f->m_n[1]->m_x,
f->m_n[2]->m_x}; f->m_n[2]->m_x};
vol = btDbvtVolume::FromPoints(points, 3); vol = btDbvtVolume::FromPoints(points, 3);
} }
vol.Expand(btVector3(pad, pad, pad)); vol.Expand(btVector3(pad, pad, pad));
@ -1253,7 +1256,8 @@ public:
{ {
updateFace(node->childs[0], use_velocity, margin); updateFace(node->childs[0], use_velocity, margin);
updateFace(node->childs[1], use_velocity, margin); updateFace(node->childs[1], use_velocity, margin);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
Merge(node->childs[0]->volume, node->childs[1]->volume, vol); Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
node->volume = vol; node->volume = vol;
} }
@ -1275,7 +1279,7 @@ public:
return (a * coord.x() + b * coord.y() + c * coord.z()); return (a * coord.x() + b * coord.y() + c * coord.z());
} }
void applyRepulsionForce(btScalar timeStep, bool applySpringForce) void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
{ {
btAlignedObjectArray<int> indices; btAlignedObjectArray<int> indices;
{ {
@ -1301,60 +1305,60 @@ public:
const btVector3& n = c.m_normal; const btVector3& n = c.m_normal;
btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w); btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w);
btScalar d = c.m_margin - n.dot(l); btScalar d = c.m_margin - n.dot(l);
d = btMax(btScalar(0),d); d = btMax(btScalar(0), d);
const btVector3& va = node->m_v; const btVector3& va = node->m_v;
btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w); btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
btVector3 vr = va - vb; btVector3 vr = va - vb;
const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing
if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
continue; continue;
btVector3 vt = vr - vn*n; btVector3 vt = vr - vn * n;
btScalar I = 0; btScalar I = 0;
btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im; btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im;
if (applySpringForce) if (applySpringForce)
I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
if (vn < 0) if (vn < 0)
I += 0.5 * mass * vn; I += 0.5 * mass * vn;
int face_penetration = 0, node_penetration = node->m_constrained; int face_penetration = 0, node_penetration = node->m_constrained;
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
face_penetration |= face->m_n[i]->m_constrained; face_penetration |= face->m_n[i]->m_constrained;
btScalar I_tilde = .5 *I /(1.0+w.length2()); btScalar I_tilde = .5 * I / (1.0 + w.length2());
// double the impulse if node or face is constrained. // double the impulse if node or face is constrained.
if (face_penetration > 0 || node_penetration > 0) if (face_penetration > 0 || node_penetration > 0)
{ {
I_tilde *= 2.0; I_tilde *= 2.0;
} }
if (face_penetration <= 0) if (face_penetration <= 0)
{ {
for (int j = 0; j < 3; ++j) for (int j = 0; j < 3; ++j)
face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im;
} }
if (node_penetration <= 0) if (node_penetration <= 0)
{ {
node->m_v -= I_tilde*node->m_im*n; node->m_v -= I_tilde * node->m_im * n;
} }
// apply frictional impulse // apply frictional impulse
btScalar vt_norm = vt.safeNorm(); btScalar vt_norm = vt.safeNorm();
if (vt_norm > SIMD_EPSILON) if (vt_norm > SIMD_EPSILON)
{ {
btScalar delta_vn = -2 * I * node->m_im; btScalar delta_vn = -2 * I * node->m_im;
btScalar mu = c.m_friction; btScalar mu = c.m_friction;
btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm; btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm;
I = 0.5 * mass * (vt_norm-vt_new); I = 0.5 * mass * (vt_norm - vt_new);
vt.safeNormalize(); vt.safeNormalize();
I_tilde = .5 *I /(1.0+w.length2()); I_tilde = .5 * I / (1.0 + w.length2());
// double the impulse if node or face is constrained. // double the impulse if node or face is constrained.
// if (face_penetration > 0 || node_penetration > 0) // if (face_penetration > 0 || node_penetration > 0)
// I_tilde *= 2.0; // I_tilde *= 2.0;
if (face_penetration <= 0) if (face_penetration <= 0)
{ {
for (int j = 0; j < 3; ++j) for (int j = 0; j < 3; ++j)
face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
} }
if (node_penetration <= 0) if (node_penetration <= 0)
{ {
node->m_v -= I_tilde * node->m_im * vt; node->m_v -= I_tilde * node->m_im * vt;
} }
@ -1362,7 +1366,7 @@ public:
} }
} }
virtual int calculateSerializeBufferSize() const; virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure) ///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
}; };

640
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@ -727,7 +727,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
int resy, int resy,
int fixeds, int fixeds,
bool gendiags, bool gendiags,
btScalar perturbation) btScalar perturbation)
{ {
#define IDX(_x_, _y_) ((_y_)*rx + (_x_)) #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
/* Create nodes */ /* Create nodes */
@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
for (int ix = 0; ix < rx; ++ix) for (int ix = 0; ix < rx; ++ix)
{ {
const btScalar tx = ix / (btScalar)(rx - 1); const btScalar tx = ix / (btScalar)(rx - 1);
btScalar pert = perturbation * btScalar(rand())/RAND_MAX; btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
btVector3 temp1 = py1; btVector3 temp1 = py1;
temp1.setY(py1.getY() + pert); temp1.setY(py1.getY() + pert);
btVector3 temp = py0; btVector3 temp = py0;
pert = perturbation * btScalar(rand())/RAND_MAX; pert = perturbation * btScalar(rand()) / RAND_MAX;
temp.setY(py0.getY() + pert); temp.setY(py0.getY() + pert);
x[IDX(ix, iy)] = lerp(temp, temp1, tx); x[IDX(ix, iy)] = lerp(temp, temp1, tx);
m[IDX(ix, iy)] = 1; m[IDX(ix, iy)] = 1;
} }
@ -1233,9 +1233,9 @@ if(face&&face[0])
} }
} }
} }
psb->initializeDmInverse(); psb->initializeDmInverse();
psb->m_tetraScratches.resize(psb->m_tetras.size()); psb->m_tetraScratches.resize(psb->m_tetras.size());
psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Nodes: %u\r\n", psb->m_nodes.size());
printf("Links: %u\r\n", psb->m_links.size()); printf("Links: %u\r\n", psb->m_links.size());
printf("Faces: %u\r\n", psb->m_faces.size()); printf("Faces: %u\r\n", psb->m_faces.size());
@ -1245,61 +1245,61 @@ if(face&&face[0])
btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
{ {
std::ifstream fs; std::ifstream fs;
fs.open(vtk_file); fs.open(vtk_file);
btAssert(fs); btAssert(fs);
typedef btAlignedObjectArray<int> Index; typedef btAlignedObjectArray<int> Index;
std::string line; std::string line;
btAlignedObjectArray<btVector3> X; btAlignedObjectArray<btVector3> X;
btVector3 position; btVector3 position;
btAlignedObjectArray<Index> indices; btAlignedObjectArray<Index> indices;
bool reading_points = false; bool reading_points = false;
bool reading_tets = false; bool reading_tets = false;
size_t n_points = 0; size_t n_points = 0;
size_t n_tets = 0; size_t n_tets = 0;
size_t x_count = 0; size_t x_count = 0;
size_t indices_count = 0; size_t indices_count = 0;
while (std::getline(fs, line)) while (std::getline(fs, line))
{ {
std::stringstream ss(line); std::stringstream ss(line);
if (line.size() == (size_t)(0)) if (line.size() == (size_t)(0))
{ {
} }
else if (line.substr(0, 6) == "POINTS") else if (line.substr(0, 6) == "POINTS")
{ {
reading_points = true; reading_points = true;
reading_tets = false; reading_tets = false;
ss.ignore(128, ' '); // ignore "POINTS" ss.ignore(128, ' '); // ignore "POINTS"
ss >> n_points; ss >> n_points;
X.resize(n_points); X.resize(n_points);
} }
else if (line.substr(0, 5) == "CELLS") else if (line.substr(0, 5) == "CELLS")
{ {
reading_points = false; reading_points = false;
reading_tets = true; reading_tets = true;
ss.ignore(128, ' '); // ignore "CELLS" ss.ignore(128, ' '); // ignore "CELLS"
ss >> n_tets; ss >> n_tets;
indices.resize(n_tets); indices.resize(n_tets);
} }
else if (line.substr(0, 10) == "CELL_TYPES") else if (line.substr(0, 10) == "CELL_TYPES")
{ {
reading_points = false; reading_points = false;
reading_tets = false; reading_tets = false;
} }
else if (reading_points) else if (reading_points)
{ {
btScalar p; btScalar p;
ss >> p; ss >> p;
position.setX(p); position.setX(p);
ss >> p; ss >> p;
position.setY(p); position.setY(p);
ss >> p; ss >> p;
position.setZ(p); position.setZ(p);
X[x_count++] = position; X[x_count++] = position;
} }
else if (reading_tets) else if (reading_tets)
{ {
int d; int d;
ss >> d; ss >> d;
if (d != 4) if (d != 4)
@ -1308,116 +1308,115 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
fs.close(); fs.close();
return 0; return 0;
} }
ss.ignore(128, ' '); // ignore "4" ss.ignore(128, ' '); // ignore "4"
Index tet; Index tet;
tet.resize(4); tet.resize(4);
for (size_t i = 0; i < 4; i++) for (size_t i = 0; i < 4; i++)
{ {
ss >> tet[i]; ss >> tet[i];
printf("%d ", tet[i]); printf("%d ", tet[i]);
} }
printf("\n"); printf("\n");
indices[indices_count++] = tet; indices[indices_count++] = tet;
} }
} }
btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
for (int i = 0; i < n_tets; ++i)
{
const Index& ni = indices[i];
psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
{
psb->appendLink(ni[0], ni[1], 0, true);
psb->appendLink(ni[1], ni[2], 0, true);
psb->appendLink(ni[2], ni[0], 0, true);
psb->appendLink(ni[0], ni[3], 0, true);
psb->appendLink(ni[1], ni[3], 0, true);
psb->appendLink(ni[2], ni[3], 0, true);
}
}
generateBoundaryFaces(psb);
psb->initializeDmInverse();
psb->m_tetraScratches.resize(psb->m_tetras.size());
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
printf("Nodes: %u\r\n", psb->m_nodes.size());
printf("Links: %u\r\n", psb->m_links.size());
printf("Faces: %u\r\n", psb->m_faces.size());
printf("Tetras: %u\r\n", psb->m_tetras.size());
fs.close(); for (int i = 0; i < n_tets; ++i)
return psb; {
const Index& ni = indices[i];
psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
{
psb->appendLink(ni[0], ni[1], 0, true);
psb->appendLink(ni[1], ni[2], 0, true);
psb->appendLink(ni[2], ni[0], 0, true);
psb->appendLink(ni[0], ni[3], 0, true);
psb->appendLink(ni[1], ni[3], 0, true);
psb->appendLink(ni[2], ni[3], 0, true);
}
}
generateBoundaryFaces(psb);
psb->initializeDmInverse();
psb->m_tetraScratches.resize(psb->m_tetras.size());
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
printf("Nodes: %u\r\n", psb->m_nodes.size());
printf("Links: %u\r\n", psb->m_links.size());
printf("Faces: %u\r\n", psb->m_faces.size());
printf("Tetras: %u\r\n", psb->m_tetras.size());
fs.close();
return psb;
} }
void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
{ {
int counter = 0; int counter = 0;
for (int i = 0; i < psb->m_nodes.size(); ++i) for (int i = 0; i < psb->m_nodes.size(); ++i)
{ {
psb->m_nodes[i].index = counter++; psb->m_nodes[i].index = counter++;
} }
typedef btAlignedObjectArray<int> Index; typedef btAlignedObjectArray<int> Index;
btAlignedObjectArray<Index> indices; btAlignedObjectArray<Index> indices;
indices.resize(psb->m_tetras.size()); indices.resize(psb->m_tetras.size());
for (int i = 0; i < indices.size(); ++i) for (int i = 0; i < indices.size(); ++i)
{ {
Index index; Index index;
index.push_back(psb->m_tetras[i].m_n[0]->index); index.push_back(psb->m_tetras[i].m_n[0]->index);
index.push_back(psb->m_tetras[i].m_n[1]->index); index.push_back(psb->m_tetras[i].m_n[1]->index);
index.push_back(psb->m_tetras[i].m_n[2]->index); index.push_back(psb->m_tetras[i].m_n[2]->index);
index.push_back(psb->m_tetras[i].m_n[3]->index); index.push_back(psb->m_tetras[i].m_n[3]->index);
indices[i] = index; indices[i] = index;
} }
std::map<std::vector<int>, std::vector<int> > dict; std::map<std::vector<int>, std::vector<int> > dict;
for (int i = 0; i < indices.size(); ++i) for (int i = 0; i < indices.size(); ++i)
{ {
for (int j = 0; j < 4; ++j) for (int j = 0; j < 4; ++j)
{ {
std::vector<int> f; std::vector<int> f;
if (j == 0) if (j == 0)
{ {
f.push_back(indices[i][1]); f.push_back(indices[i][1]);
f.push_back(indices[i][0]); f.push_back(indices[i][0]);
f.push_back(indices[i][2]); f.push_back(indices[i][2]);
} }
if (j == 1) if (j == 1)
{ {
f.push_back(indices[i][3]); f.push_back(indices[i][3]);
f.push_back(indices[i][0]); f.push_back(indices[i][0]);
f.push_back(indices[i][1]); f.push_back(indices[i][1]);
} }
if (j == 2) if (j == 2)
{ {
f.push_back(indices[i][3]); f.push_back(indices[i][3]);
f.push_back(indices[i][1]); f.push_back(indices[i][1]);
f.push_back(indices[i][2]); f.push_back(indices[i][2]);
} }
if (j == 3) if (j == 3)
{ {
f.push_back(indices[i][2]); f.push_back(indices[i][2]);
f.push_back(indices[i][0]); f.push_back(indices[i][0]);
f.push_back(indices[i][3]); f.push_back(indices[i][3]);
} }
std::vector<int> f_sorted = f; std::vector<int> f_sorted = f;
std::sort(f_sorted.begin(), f_sorted.end()); std::sort(f_sorted.begin(), f_sorted.end());
if (dict.find(f_sorted) != dict.end()) if (dict.find(f_sorted) != dict.end())
{ {
dict.erase(f_sorted); dict.erase(f_sorted);
} }
else else
{ {
dict.insert(std::make_pair(f_sorted, f)); dict.insert(std::make_pair(f_sorted, f));
} }
} }
} }
for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
{ {
std::vector<int> f = it->second; std::vector<int> f = it->second;
psb->appendFace(f[0], f[1], f[2]); psb->appendFace(f[0], f[1], f[2]);
} }
} }
//Write the surface mesh to an obj file. //Write the surface mesh to an obj file.
@ -1429,7 +1428,7 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
if (psb->m_tetras.size() > 0) if (psb->m_tetras.size() > 0)
{ {
// For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/
std::map<int, int> dict; std::map<int, int> dict;
for (int i = 0; i < psb->m_faces.size(); i++) for (int i = 0; i < psb->m_faces.size(); i++)
{ {
@ -1449,7 +1448,7 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
} }
} }
} }
// Write surface mesh. // Write surface mesh.
for (int i = 0; i < psb->m_faces.size(); ++i) for (int i = 0; i < psb->m_faces.size(); ++i)
{ {
fs << "f"; fs << "f";
@ -1462,7 +1461,7 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
} }
else else
{ {
// For trimesh, directly write out all the nodes and faces.xs // For trimesh, directly write out all the nodes and faces.xs
for (int i = 0; i < psb->m_nodes.size(); ++i) for (int i = 0; i < psb->m_nodes.size(); ++i)
{ {
fs << "v"; fs << "v";
@ -1488,176 +1487,175 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
{ {
std::ifstream fs_read; std::ifstream fs_read;
fs_read.open(filename); fs_read.open(filename);
std::string line; std::string line;
btVector3 pos; btVector3 pos;
btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
while (std::getline(fs_read, line)) while (std::getline(fs_read, line))
{ {
std::stringstream ss(line); std::stringstream ss(line);
if (line[0] == 'v') if (line[0] == 'v')
{ {
} }
else if (line[0] == 'f') else if (line[0] == 'f')
{ {
ss.ignore(); ss.ignore();
int id0, id1, id2; int id0, id1, id2;
ss >> id0; ss >> id0;
ss >> id1; ss >> id1;
ss >> id2; ss >> id2;
btAlignedObjectArray<int> new_face; btAlignedObjectArray<int> new_face;
new_face.push_back(id1); new_face.push_back(id1);
new_face.push_back(id0); new_face.push_back(id0);
new_face.push_back(id2); new_face.push_back(id2);
additional_faces.push_back(new_face); additional_faces.push_back(new_face);
} }
} }
fs_read.close(); fs_read.close();
std::ofstream fs_write; std::ofstream fs_write;
fs_write.open(filename, std::ios_base::app); fs_write.open(filename, std::ios_base::app);
for (int i = 0; i < additional_faces.size(); ++i) for (int i = 0; i < additional_faces.size(); ++i)
{ {
fs_write << "f"; fs_write << "f";
for (int n = 0; n < 3; n++) for (int n = 0; n < 3; n++)
{ {
fs_write << " " << additional_faces[i][n]; fs_write << " " << additional_faces[i][n];
} }
fs_write << "\n"; fs_write << "\n";
} }
fs_write.close(); fs_write.close();
} }
// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
{ {
btVector3 vap = p - a; btVector3 vap = p - a;
btVector3 vbp = p - b; btVector3 vbp = p - b;
btVector3 vab = b - a; btVector3 vab = b - a;
btVector3 vac = c - a; btVector3 vac = c - a;
btVector3 vad = d - a; btVector3 vad = d - a;
btVector3 vbc = c - b; btVector3 vbc = c - b;
btVector3 vbd = d - b; btVector3 vbd = d - b;
btScalar va6 = (vbp.cross(vbd)).dot(vbc); btScalar va6 = (vbp.cross(vbd)).dot(vbc);
btScalar vb6 = (vap.cross(vac)).dot(vad); btScalar vb6 = (vap.cross(vac)).dot(vad);
btScalar vc6 = (vap.cross(vad)).dot(vab); btScalar vc6 = (vap.cross(vad)).dot(vab);
btScalar vd6 = (vap.cross(vab)).dot(vac); btScalar vd6 = (vap.cross(vab)).dot(vac);
btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
} }
// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
{ {
btVector3 v0 = b - a, v1 = c - a, v2 = p - a; btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
btScalar d00 = btDot(v0, v0); btScalar d00 = btDot(v0, v0);
btScalar d01 = btDot(v0, v1); btScalar d01 = btDot(v0, v1);
btScalar d11 = btDot(v1, v1); btScalar d11 = btDot(v1, v1);
btScalar d20 = btDot(v2, v0); btScalar d20 = btDot(v2, v0);
btScalar d21 = btDot(v2, v1); btScalar d21 = btDot(v2, v1);
btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
bary[1] = (d11 * d20 - d01 * d21) * invDenom; bary[1] = (d11 * d20 - d01 * d21) * invDenom;
bary[2] = (d00 * d21 - d01 * d20) * invDenom; bary[2] = (d00 * d21 - d01 * d20) * invDenom;
bary[0] = 1.0 - bary[1] - bary[2]; bary[0] = 1.0 - bary[1] - bary[2];
bary[3] = 0; bary[3] = 0;
} }
// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
{ {
psb->m_z.resize(0); psb->m_z.resize(0);
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
for (int i = 0; i < psb->m_renderNodes.size(); ++i) for (int i = 0; i < psb->m_renderNodes.size(); ++i)
{ {
const btVector3& p = psb->m_renderNodes[i].m_x; const btVector3& p = psb->m_renderNodes[i].m_x;
btVector4 bary; btVector4 bary;
btVector4 optimal_bary; btVector4 optimal_bary;
btScalar min_bary_weight = -1e3; btScalar min_bary_weight = -1e3;
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
for (int j = 0; j < psb->m_tetras.size(); ++j) for (int j = 0; j < psb->m_tetras.size(); ++j)
{ {
const btSoftBody::Tetra& t = psb->m_tetras[j]; const btSoftBody::Tetra& t = psb->m_tetras[j];
getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
btScalar new_min_bary_weight = bary[0]; btScalar new_min_bary_weight = bary[0];
for (int k = 1; k < 4; ++k) for (int k = 1; k < 4; ++k)
{ {
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
} }
if (new_min_bary_weight > min_bary_weight) if (new_min_bary_weight > min_bary_weight)
{ {
btAlignedObjectArray<const btSoftBody::Node*> parents; btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(t.m_n[0]); parents.push_back(t.m_n[0]);
parents.push_back(t.m_n[1]); parents.push_back(t.m_n[1]);
parents.push_back(t.m_n[2]); parents.push_back(t.m_n[2]);
parents.push_back(t.m_n[3]); parents.push_back(t.m_n[3]);
optimal_parents = parents; optimal_parents = parents;
optimal_bary = bary; optimal_bary = bary;
min_bary_weight = new_min_bary_weight; min_bary_weight = new_min_bary_weight;
// stop searching if p is inside the tetrahedron at hand // stop searching if p is inside the tetrahedron at hand
if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.) if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
{ {
break; break;
} }
} }
} }
psb->m_renderNodesInterpolationWeights[i] = optimal_bary; psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
psb->m_renderNodesParents[i] = optimal_parents; psb->m_renderNodesParents[i] = optimal_parents;
} }
} }
// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
{ {
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
psb->m_z.resize(psb->m_renderNodes.size()); psb->m_z.resize(psb->m_renderNodes.size());
for (int i = 0; i < psb->m_renderNodes.size(); ++i) for (int i = 0; i < psb->m_renderNodes.size(); ++i)
{ {
const btVector3& p = psb->m_renderNodes[i].m_x; const btVector3& p = psb->m_renderNodes[i].m_x;
btVector4 bary; btVector4 bary;
btVector4 optimal_bary; btVector4 optimal_bary;
btScalar min_bary_weight = -SIMD_INFINITY; btScalar min_bary_weight = -SIMD_INFINITY;
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
btScalar dist = 0, optimal_dist = 0; btScalar dist = 0, optimal_dist = 0;
for (int j = 0; j < psb->m_faces.size(); ++j) for (int j = 0; j < psb->m_faces.size(); ++j)
{ {
const btSoftBody::Face& f = psb->m_faces[j]; const btSoftBody::Face& f = psb->m_faces[j];
btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
btVector3 unit_n = n.normalized(); btVector3 unit_n = n.normalized();
dist = (p-f.m_n[0]->m_x).dot(unit_n); dist = (p - f.m_n[0]->m_x).dot(unit_n);
btVector3 proj_p = p - dist*unit_n; btVector3 proj_p = p - dist * unit_n;
getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
btScalar new_min_bary_weight = bary[0]; btScalar new_min_bary_weight = bary[0];
for (int k = 1; k < 3; ++k) for (int k = 1; k < 3; ++k)
{ {
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
} }
// p is out of the current best triangle, we found a traingle that's better // p is out of the current best triangle, we found a traingle that's better
bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.); bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
// p is inside of the current best triangle, we found a triangle that's better // p is inside of the current best triangle, we found a triangle that's better
bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist)); bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
if (better_than_closest_outisde || better_than_best_inside) if (better_than_closest_outisde || better_than_best_inside)
{ {
btAlignedObjectArray<const btSoftBody::Node*> parents; btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(f.m_n[0]); parents.push_back(f.m_n[0]);
parents.push_back(f.m_n[1]); parents.push_back(f.m_n[1]);
parents.push_back(f.m_n[2]); parents.push_back(f.m_n[2]);
optimal_parents = parents; optimal_parents = parents;
optimal_bary = bary; optimal_bary = bary;
optimal_dist = dist; optimal_dist = dist;
min_bary_weight = new_min_bary_weight; min_bary_weight = new_min_bary_weight;
} }
} }
psb->m_renderNodesInterpolationWeights[i] = optimal_bary; psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
psb->m_renderNodesParents[i] = optimal_parents; psb->m_renderNodesParents[i] = optimal_parents;
psb->m_z[i] = optimal_dist; psb->m_z[i] = optimal_dist;
} }
} }

30
src/BulletSoftBody/btSoftBodyHelpers.h
View File

@ -93,7 +93,7 @@ struct btSoftBodyHelpers
int resy, int resy,
int fixeds, int fixeds,
bool gendiags, bool gendiags,
btScalar perturbation = 0.); btScalar perturbation = 0.);
/* Create a patch with UV Texture Coordinates */ /* Create a patch with UV Texture Coordinates */
static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00, const btVector3& corner00,
@ -142,21 +142,21 @@ struct btSoftBodyHelpers
bool bfacelinks, bool bfacelinks,
bool btetralinks, bool btetralinks,
bool bfacesfromtetras); bool bfacesfromtetras);
static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
static void writeObj(const char* file, const btSoftBody* psb); static void writeObj(const char* file, const btSoftBody* psb);
static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
static void interpolateBarycentricWeights(btSoftBody* psb); static void interpolateBarycentricWeights(btSoftBody* psb);
static void extrapolateBarycentricWeights(btSoftBody* psb); static void extrapolateBarycentricWeights(btSoftBody* psb);
static void generateBoundaryFaces(btSoftBody* psb); static void generateBoundaryFaces(btSoftBody* psb);
static void duplicateFaces(const char* filename, const btSoftBody* psb); static void duplicateFaces(const char* filename, const btSoftBody* psb);
/// Sort the list of links to move link calculations that are dependent upon earlier /// Sort the list of links to move link calculations that are dependent upon earlier
/// ones as far as possible away from the calculation of those values /// ones as far as possible away from the calculation of those values
/// This tends to make adjacent loop iterations not dependent upon one another, /// This tends to make adjacent loop iterations not dependent upon one another,

2029
src/BulletSoftBody/btSoftBodyInternals.h
View File

File diff suppressed because it is too large Load Diff

2
src/BulletSoftBody/btSoftBodySolvers.h
View File

@ -36,7 +36,7 @@ public:
CL_SIMD_SOLVER, CL_SIMD_SOLVER,
DX_SOLVER, DX_SOLVER,
DX_SIMD_SOLVER, DX_SIMD_SOLVER,
DEFORMABLE_SOLVER DEFORMABLE_SOLVER
}; };
protected: protected:

68
src/BulletSoftBody/btSparseSDF.h
View File

@ -22,36 +22,36 @@ subject to the following restrictions:
// Fast Hash // Fast Hash
#if !defined (get16bits) #if !defined(get16bits)
#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\ #define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
+(unsigned int)(((const unsigned char *)(d))[0]) )
#endif #endif
// //
// super hash function by Paul Hsieh // super hash function by Paul Hsieh
// //
inline unsigned int HsiehHash (const char * data, int len) { inline unsigned int HsiehHash(const char* data, int len)
unsigned int hash = len, tmp; {
len>>=2; unsigned int hash = len, tmp;
len >>= 2;
/* Main loop */ /* Main loop */
for (;len > 0; len--) { for (; len > 0; len--)
hash += get16bits (data); {
tmp = (get16bits (data+2) << 11) ^ hash; hash += get16bits(data);
hash = (hash << 16) ^ tmp; tmp = (get16bits(data + 2) << 11) ^ hash;
data += 2*sizeof (unsigned short); hash = (hash << 16) ^ tmp;
hash += hash >> 11; data += 2 * sizeof(unsigned short);
} hash += hash >> 11;
}
/* Force "avalanching" of final 127 bits */
hash ^= hash << 3;
hash += hash >> 5;
hash ^= hash << 4;
hash += hash >> 17;
hash ^= hash << 25;
hash += hash >> 6;
/* Force "avalanching" of final 127 bits */ return hash;
hash ^= hash << 3;
hash += hash >> 5;
hash ^= hash << 4;
hash += hash >> 17;
hash ^= hash << 25;
hash += hash >> 6;
return hash;
} }
template <const int CELLSIZE> template <const int CELLSIZE>
@ -81,7 +81,7 @@ struct btSparseSdf
btAlignedObjectArray<Cell*> cells; btAlignedObjectArray<Cell*> cells;
btScalar voxelsz; btScalar voxelsz;
btScalar m_defaultVoxelsz; btScalar m_defaultVoxelsz;
int puid; int puid;
int ncells; int ncells;
int m_clampCells; int m_clampCells;
@ -103,16 +103,16 @@ struct btSparseSdf
//if this limit is reached, the SDF is reset (at the cost of some performance during the reset) //if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
m_clampCells = clampCells; m_clampCells = clampCells;
cells.resize(hashsize, 0); cells.resize(hashsize, 0);
m_defaultVoxelsz = 0.25; m_defaultVoxelsz = 0.25;
Reset(); Reset();
} }
// //
void setDefaultVoxelsz(btScalar sz) void setDefaultVoxelsz(btScalar sz)
{ {
m_defaultVoxelsz = sz; m_defaultVoxelsz = sz;
} }
void Reset() void Reset()
{ {
for (int i = 0, ni = cells.size(); i < ni; ++i) for (int i = 0, ni = cells.size(); i < ni; ++i)
@ -162,7 +162,7 @@ struct btSparseSdf
nqueries = 1; nqueries = 1;
nprobes = 1; nprobes = 1;
++puid; ///@todo: Reset puid's when int range limit is reached */ ++puid; ///@todo: Reset puid's when int range limit is reached */
/* else setup a priority list... */ /* else setup a priority list... */
} }
// //
int RemoveReferences(btCollisionShape* pcs) int RemoveReferences(btCollisionShape* pcs)
@ -221,7 +221,7 @@ struct btSparseSdf
else else
{ {
// printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
//printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
c = c->next; c = c->next;
} }
@ -363,7 +363,7 @@ struct btSparseSdf
myset.p = (void*)shape; myset.p = (void*)shape;
const char* ptr = (const char*)&myset; const char* ptr = (const char*)&myset;
unsigned int result = HsiehHash(ptr, sizeof(btS) ); unsigned int result = HsiehHash(ptr, sizeof(btS));
return result; return result;
} }

742
src/BulletSoftBody/poly34.cpp
View File

@ -6,7 +6,7 @@
// //
#include <math.h> #include <math.h>
#include "poly34.h" // solution of cubic and quartic equation #include "poly34.h" // solution of cubic and quartic equation
#define TwoPi 6.28318530717958648 #define TwoPi 6.28318530717958648
const btScalar eps = SIMD_EPSILON; const btScalar eps = SIMD_EPSILON;
@ -15,50 +15,53 @@ const btScalar eps = SIMD_EPSILON;
//============================================================================= //=============================================================================
static SIMD_FORCE_INLINE btScalar _root3(btScalar x) static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
{ {
btScalar s = 1.; btScalar s = 1.;
while (x < 1.) { while (x < 1.)
x *= 8.; {
s *= 0.5; x *= 8.;
} s *= 0.5;
while (x > 8.) { }
x *= 0.125; while (x > 8.)
s *= 2.; {
} x *= 0.125;
btScalar r = 1.5; s *= 2.;
r -= 1. / 3. * (r - x / (r * r)); }
r -= 1. / 3. * (r - x / (r * r)); btScalar r = 1.5;
r -= 1. / 3. * (r - x / (r * r)); r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r)); r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r)); r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r)); r -= 1. / 3. * (r - x / (r * r));
return r * s; r -= 1. / 3. * (r - x / (r * r));
r -= 1. / 3. * (r - x / (r * r));
return r * s;
} }
btScalar SIMD_FORCE_INLINE root3(btScalar x) btScalar SIMD_FORCE_INLINE root3(btScalar x)
{ {
if (x > 0) if (x > 0)
return _root3(x); return _root3(x);
else if (x < 0) else if (x < 0)
return -_root3(-x); return -_root3(-x);
else else
return 0.; return 0.;
} }
// x - array of size 2 // x - array of size 2
// return 2: 2 real roots x[0], x[1] // return 2: 2 real roots x[0], x[1]
// return 0: pair of complex roots: x[0]i*x[1] // return 0: pair of complex roots: x[0]i*x[1]
int SolveP2(btScalar* x, btScalar a, btScalar b) int SolveP2(btScalar* x, btScalar a, btScalar b)
{ // solve equation x^2 + a*x + b = 0 { // solve equation x^2 + a*x + b = 0
btScalar D = 0.25 * a * a - b; btScalar D = 0.25 * a * a - b;
if (D >= 0) { if (D >= 0)
D = sqrt(D); {
x[0] = -0.5 * a + D; D = sqrt(D);
x[1] = -0.5 * a - D; x[0] = -0.5 * a + D;
return 2; x[1] = -0.5 * a - D;
} return 2;
x[0] = -0.5 * a; }
x[1] = sqrt(-D); x[0] = -0.5 * a;
return 0; x[1] = sqrt(-D);
return 0;
} }
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
// x - array of size 3 // x - array of size 3
@ -66,217 +69,228 @@ int SolveP2(btScalar* x, btScalar a, btScalar b)
// 2 real roots: x[0], x[1], return 2 // 2 real roots: x[0], x[1], return 2
// 1 real root : x[0], x[1] i*x[2], return 1 // 1 real root : x[0], x[1] i*x[2], return 1
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c) int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 { // solve cubic equation x^3 + a*x^2 + b*x + c = 0
btScalar a2 = a * a; btScalar a2 = a * a;
btScalar q = (a2 - 3 * b) / 9; btScalar q = (a2 - 3 * b) / 9;
if (q < 0) if (q < 0)
q = eps; q = eps;
btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
// equation x^3 + q*x + r = 0 // equation x^3 + q*x + r = 0
btScalar r2 = r * r; btScalar r2 = r * r;
btScalar q3 = q * q * q; btScalar q3 = q * q * q;
btScalar A, B; btScalar A, B;
if (r2 <= (q3 + eps)) { //<<-- FIXED! if (r2 <= (q3 + eps))
btScalar t = r / sqrt(q3); { //<<-- FIXED!
if (t < -1) btScalar t = r / sqrt(q3);
t = -1; if (t < -1)
if (t > 1) t = -1;
t = 1; if (t > 1)
t = acos(t); t = 1;
a /= 3; t = acos(t);
q = -2 * sqrt(q); a /= 3;
x[0] = q * cos(t / 3) - a; q = -2 * sqrt(q);
x[1] = q * cos((t + TwoPi) / 3) - a; x[0] = q * cos(t / 3) - a;
x[2] = q * cos((t - TwoPi) / 3) - a; x[1] = q * cos((t + TwoPi) / 3) - a;
return (3); x[2] = q * cos((t - TwoPi) / 3) - a;
} return (3);
else { }
//A =-pow(fabs(r)+sqrt(r2-q3),1./3); else
A = -root3(fabs(r) + sqrt(r2 - q3)); {
if (r < 0) //A =-pow(fabs(r)+sqrt(r2-q3),1./3);
A = -A; A = -root3(fabs(r) + sqrt(r2 - q3));
B = (A == 0 ? 0 : q / A); if (r < 0)
A = -A;
a /= 3; B = (A == 0 ? 0 : q / A);
x[0] = (A + B) - a;
x[1] = -0.5 * (A + B) - a; a /= 3;
x[2] = 0.5 * sqrt(3.) * (A - B); x[0] = (A + B) - a;
if (fabs(x[2]) < eps) { x[1] = -0.5 * (A + B) - a;
x[2] = x[1]; x[2] = 0.5 * sqrt(3.) * (A - B);
return (2); if (fabs(x[2]) < eps)
} {
return (1); x[2] = x[1];
} return (2);
} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { }
return (1);
}
} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
// a>=0! // a>=0!
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y)
{ {
btScalar r = sqrt(x * x + y * y); btScalar r = sqrt(x * x + y * y);
if (y == 0) { if (y == 0)
r = sqrt(r); {
if (x >= 0) { r = sqrt(r);
a = r; if (x >= 0)
b = 0; {
} a = r;
else { b = 0;
a = 0; }
b = r; else
} {
} a = 0;
else { // y != 0 b = r;
a = sqrt(0.5 * (x + r)); }
b = 0.5 * y / a; }
} else
{ // y != 0
a = sqrt(0.5 * (x + r));
b = 0.5 * y / a;
}
} }
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0
{ {
btScalar D = b * b - 4 * d; btScalar D = b * b - 4 * d;
if (D >= 0) { if (D >= 0)
btScalar sD = sqrt(D); {
btScalar x1 = (-b + sD) / 2; btScalar sD = sqrt(D);
btScalar x2 = (-b - sD) / 2; // x2 <= x1 btScalar x1 = (-b + sD) / 2;
if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots btScalar x2 = (-b - sD) / 2; // x2 <= x1
{ if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots
btScalar sx1 = sqrt(x1); {
btScalar sx2 = sqrt(x2); btScalar sx1 = sqrt(x1);
x[0] = -sx1; btScalar sx2 = sqrt(x2);
x[1] = sx1; x[0] = -sx1;
x[2] = -sx2; x[1] = sx1;
x[3] = sx2; x[2] = -sx2;
return 4; x[3] = sx2;
} return 4;
if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots }
{ if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots
btScalar sx1 = sqrt(-x1); {
btScalar sx2 = sqrt(-x2); btScalar sx1 = sqrt(-x1);
x[0] = 0; btScalar sx2 = sqrt(-x2);
x[1] = sx1; x[0] = 0;
x[2] = 0; x[1] = sx1;
x[3] = sx2; x[2] = 0;
return 0; x[3] = sx2;
} return 0;
// now x2 < 0 <= x1 , two real roots and one pair of imginary root }
btScalar sx1 = sqrt(x1); // now x2 < 0 <= x1 , two real roots and one pair of imginary root
btScalar sx2 = sqrt(-x2); btScalar sx1 = sqrt(x1);
x[0] = -sx1; btScalar sx2 = sqrt(-x2);
x[1] = sx1; x[0] = -sx1;
x[2] = 0; x[1] = sx1;
x[3] = sx2; x[2] = 0;
return 2; x[3] = sx2;
} return 2;
else { // if( D < 0 ), two pair of compex roots }
btScalar sD2 = 0.5 * sqrt(-D); else
CSqrt(-0.5 * b, sD2, x[0], x[1]); { // if( D < 0 ), two pair of compex roots
CSqrt(-0.5 * b, -sD2, x[2], x[3]); btScalar sD2 = 0.5 * sqrt(-D);
return 0; CSqrt(-0.5 * b, sD2, x[0], x[1]);
} // if( D>=0 ) CSqrt(-0.5 * b, -sD2, x[2], x[3]);
} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d return 0;
} // if( D>=0 )
} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
#define SWAP(a, b) \ #define SWAP(a, b) \
{ \ { \
t = b; \ t = b; \
b = a; \ b = a; \
a = t; \ a = t; \
} }
static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c
{ {
btScalar t; btScalar t;
if (a > b) if (a > b)
SWAP(a, b); // now a<=b SWAP(a, b); // now a<=b
if (c < b) { if (c < b)
SWAP(b, c); // now a<=b, b<=c {
if (a > b) SWAP(b, c); // now a<=b, b<=c
SWAP(a, b); // now a<=b if (a > b)
} SWAP(a, b); // now a<=b
}
} }
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
{ {
//if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
return SolveP4Bi(x, b, d); // After that, c!=0 return SolveP4Bi(x, b, d); // After that, c!=0
int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
// by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
if (res3 > 1) // 3 real roots, if (res3 > 1) // 3 real roots,
{ {
dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
// Note: x[0]*x[1]*x[2]= c*c > 0 // Note: x[0]*x[1]*x[2]= c*c > 0
if (x[0] > 0) // all roots are positive if (x[0] > 0) // all roots are positive
{ {
btScalar sz1 = sqrt(x[0]); btScalar sz1 = sqrt(x[0]);
btScalar sz2 = sqrt(x[1]); btScalar sz2 = sqrt(x[1]);
btScalar sz3 = sqrt(x[2]); btScalar sz3 = sqrt(x[2]);
// Note: sz1*sz2*sz3= -c (and not equal to 0) // Note: sz1*sz2*sz3= -c (and not equal to 0)
if (c > 0) { if (c > 0)
x[0] = (-sz1 - sz2 - sz3) / 2; {
x[1] = (-sz1 + sz2 + sz3) / 2; x[0] = (-sz1 - sz2 - sz3) / 2;
x[2] = (+sz1 - sz2 + sz3) / 2; x[1] = (-sz1 + sz2 + sz3) / 2;
x[3] = (+sz1 + sz2 - sz3) / 2; x[2] = (+sz1 - sz2 + sz3) / 2;
return 4; x[3] = (+sz1 + sz2 - sz3) / 2;
} return 4;
// now: c<0 }
x[0] = (-sz1 - sz2 + sz3) / 2; // now: c<0
x[1] = (-sz1 + sz2 - sz3) / 2; x[0] = (-sz1 - sz2 + sz3) / 2;
x[2] = (+sz1 - sz2 - sz3) / 2; x[1] = (-sz1 + sz2 - sz3) / 2;
x[3] = (+sz1 + sz2 + sz3) / 2; x[2] = (+sz1 - sz2 - sz3) / 2;
return 4; x[3] = (+sz1 + sz2 + sz3) / 2;
} // if( x[0] > 0) // all roots are positive return 4;
// now x[0] <= x[1] < 0, x[2] > 0 } // if( x[0] > 0) // all roots are positive
// two pair of comlex roots // now x[0] <= x[1] < 0, x[2] > 0
btScalar sz1 = sqrt(-x[0]); // two pair of comlex roots
btScalar sz2 = sqrt(-x[1]); btScalar sz1 = sqrt(-x[0]);
btScalar sz3 = sqrt(x[2]); btScalar sz2 = sqrt(-x[1]);
btScalar sz3 = sqrt(x[2]);
if (c > 0) // sign = -1
{ if (c > 0) // sign = -1
x[0] = -sz3 / 2; {
x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] x[0] = -sz3 / 2;
x[2] = sz3 / 2; x[1] = (sz1 - sz2) / 2; // x[0]i*x[1]
x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] x[2] = sz3 / 2;
return 0; x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3]
} return 0;
// now: c<0 , sign = +1 }
x[0] = sz3 / 2; // now: c<0 , sign = +1
x[1] = (-sz1 + sz2) / 2; x[0] = sz3 / 2;
x[2] = -sz3 / 2; x[1] = (-sz1 + sz2) / 2;
x[3] = (sz1 + sz2) / 2; x[2] = -sz3 / 2;
return 0; x[3] = (sz1 + sz2) / 2;
} // if( res3>1 ) // 3 real roots, return 0;
// now resoventa have 1 real and pair of compex roots } // if( res3>1 ) // 3 real roots,
// x[0] - real root, and x[0]>0, // now resoventa have 1 real and pair of compex roots
// x[1]i*x[2] - complex roots, // x[0] - real root, and x[0]>0,
// x[0] must be >=0. But one times x[0]=~ 1e-17, so: // x[1]i*x[2] - complex roots,
if (x[0] < 0) // x[0] must be >=0. But one times x[0]=~ 1e-17, so:
x[0] = 0; if (x[0] < 0)
btScalar sz1 = sqrt(x[0]); x[0] = 0;
btScalar szr, szi; btScalar sz1 = sqrt(x[0]);
CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] btScalar szr, szi;
if (c > 0) // sign = -1 CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
{ if (c > 0) // sign = -1
x[0] = -sz1 / 2 - szr; // 1st real root {
x[1] = -sz1 / 2 + szr; // 2nd real root x[0] = -sz1 / 2 - szr; // 1st real root
x[2] = sz1 / 2; x[1] = -sz1 / 2 + szr; // 2nd real root
x[3] = szi; x[2] = sz1 / 2;
return 2; x[3] = szi;
} return 2;
// now: c<0 , sign = +1 }
x[0] = sz1 / 2 - szr; // 1st real root // now: c<0 , sign = +1
x[1] = sz1 / 2 + szr; // 2nd real root x[0] = sz1 / 2 - szr; // 1st real root
x[2] = -sz1 / 2; x[1] = sz1 / 2 + szr; // 2nd real root
x[3] = szi; x[2] = -sz1 / 2;
return 2; x[3] = szi;
} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d return 2;
} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
{ {
btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
if (fxs == 0) if (fxs == 0)
return x; //return 1e99; <<-- FIXED! return x; //return 1e99; <<-- FIXED!
btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
return x - fx / fxs; return x - fx / fxs;
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
// x - array of size 4 // x - array of size 4
@ -284,136 +298,150 @@ btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // o
// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d) int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method { // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
// move to a=0: // move to a=0:
btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
btScalar b1 = b - 0.375 * a * a; btScalar b1 = b - 0.375 * a * a;
int res = SolveP4De(x, b1, c1, d1); int res = SolveP4De(x, b1, c1, d1);
if (res == 4) { if (res == 4)
x[0] -= a / 4; {
x[1] -= a / 4; x[0] -= a / 4;
x[2] -= a / 4; x[1] -= a / 4;
x[3] -= a / 4; x[2] -= a / 4;
} x[3] -= a / 4;
else if (res == 2) { }
x[0] -= a / 4; else if (res == 2)
x[1] -= a / 4; {
x[2] -= a / 4; x[0] -= a / 4;
} x[1] -= a / 4;
else { x[2] -= a / 4;
x[0] -= a / 4; }
x[2] -= a / 4; else
} {
// one Newton step for each real root: x[0] -= a / 4;
if (res > 0) { x[2] -= a / 4;
x[0] = N4Step(x[0], a, b, c, d); }
x[1] = N4Step(x[1], a, b, c, d); // one Newton step for each real root:
} if (res > 0)
if (res > 2) { {
x[2] = N4Step(x[2], a, b, c, d); x[0] = N4Step(x[0], a, b, c, d);
x[3] = N4Step(x[3], a, b, c, d); x[1] = N4Step(x[1], a, b, c, d);
} }
return res; if (res > 2)
{
x[2] = N4Step(x[2], a, b, c, d);
x[3] = N4Step(x[3], a, b, c, d);
}
return res;
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e) #define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
{ {
int cnt; int cnt;
if (fabs(e) < eps) if (fabs(e) < eps)
return 0; return 0;
btScalar brd = fabs(a); // brd - border of real roots btScalar brd = fabs(a); // brd - border of real roots
if (fabs(b) > brd) if (fabs(b) > brd)
brd = fabs(b); brd = fabs(b);
if (fabs(c) > brd) if (fabs(c) > brd)
brd = fabs(c); brd = fabs(c);
if (fabs(d) > brd) if (fabs(d) > brd)
brd = fabs(d); brd = fabs(d);
if (fabs(e) > brd) if (fabs(e) > brd)
brd = fabs(e); brd = fabs(e);
brd++; // brd - border of real roots brd++; // brd - border of real roots
btScalar x0, f0; // less than root btScalar x0, f0; // less than root
btScalar x1, f1; // greater than root btScalar x1, f1; // greater than root
btScalar x2, f2, f2s; // next values, f(x2), f'(x2) btScalar x2, f2, f2s; // next values, f(x2), f'(x2)
btScalar dx = 0; btScalar dx = 0;
if (e < 0) { if (e < 0)
x0 = 0; {
x1 = brd; x0 = 0;
f0 = e; x1 = brd;
f1 = F5(x1); f0 = e;
x2 = 0.01 * brd; f1 = F5(x1);
} // positive root x2 = 0.01 * brd;
else { } // positive root
x0 = -brd; else
x1 = 0; {
f0 = F5(x0); x0 = -brd;
f1 = e; x1 = 0;
x2 = -0.01 * brd; f0 = F5(x0);
} // negative root f1 = e;
x2 = -0.01 * brd;
if (fabs(f0) < eps) } // negative root
return x0;
if (fabs(f1) < eps) if (fabs(f0) < eps)
return x1; return x0;
if (fabs(f1) < eps)
// now x0<x1, f(x0)<0, f(x1)>0 return x1;
// Firstly 10 bisections
for (cnt = 0; cnt < 10; cnt++) { // now x0<x1, f(x0)<0, f(x1)>0
x2 = (x0 + x1) / 2; // next point // Firstly 10 bisections
//x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point for (cnt = 0; cnt < 10; cnt++)
f2 = F5(x2); // f(x2) {
if (fabs(f2) < eps) x2 = (x0 + x1) / 2; // next point
return x2; //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point
if (f2 > 0) { f2 = F5(x2); // f(x2)
x1 = x2; if (fabs(f2) < eps)
f1 = f2; return x2;
} if (f2 > 0)
else { {
x0 = x2; x1 = x2;
f0 = f2; f1 = f2;
} }
} else
{
// At each step: x0 = x2;
// x0<x1, f(x0)<0, f(x1)>0. f0 = f2;
// x2 - next value }
// we hope that x0 < x2 < x1, but not necessarily }
do {
if (cnt++ > 50) // At each step:
break; // x0<x1, f(x0)<0, f(x1)>0.
if (x2 <= x0 || x2 >= x1) // x2 - next value
x2 = (x0 + x1) / 2; // now x0 < x2 < x1 // we hope that x0 < x2 < x1, but not necessarily
f2 = F5(x2); // f(x2) do
if (fabs(f2) < eps) {
return x2; if (cnt++ > 50)
if (f2 > 0) { break;
x1 = x2; if (x2 <= x0 || x2 >= x1)
f1 = f2; x2 = (x0 + x1) / 2; // now x0 < x2 < x1
} f2 = F5(x2); // f(x2)
else { if (fabs(f2) < eps)
x0 = x2; return x2;
f0 = f2; if (f2 > 0)
} {
f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) x1 = x2;
if (fabs(f2s) < eps) { f1 = f2;
x2 = 1e99; }
continue; else
} {
dx = f2 / f2s; x0 = x2;
x2 -= dx; f0 = f2;
} while (fabs(dx) > eps); }
return x2; f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 if (fabs(f2s) < eps)
{
x2 = 1e99;
continue;
}
dx = f2 / f2s;
x2 -= dx;
} while (fabs(dx) > eps);
return x2;
} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
{ {
btScalar r = x[0] = SolveP5_1(a, b, c, d, e); btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
return 1 + SolveP4(x + 1, a1, b1, c1, d1); return 1 + SolveP4(x + 1, a1, b1, c1, d1);
} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 } // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------

18
src/BulletSoftBody/poly34.h
View File

@ -8,31 +8,31 @@
// x - array of size 2 // x - array of size 2
// return 2: 2 real roots x[0], x[1] // return 2: 2 real roots x[0], x[1]
// return 0: pair of complex roots: x[0]i*x[1] // return 0: pair of complex roots: x[0]i*x[1]
int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0
// x - array of size 3 // x - array of size 3
// return 3: 3 real roots x[0], x[1], x[2] // return 3: 3 real roots x[0], x[1], x[2]
// return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2] // return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2]
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0
// x - array of size 4 // x - array of size 4
// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
// x - array of size 5 // x - array of size 5
// return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots // return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots
// return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4], // return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4],
// return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4], // return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4],
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
// And some additional functions for internal use. // And some additional functions for internal use.
// Your may remove this definitions from here // Your may remove this definitions from here
int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y)
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
#endif #endif