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.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;
}

334
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -93,11 +93,10 @@
#include "../TinyAudio/b3SoundEngine.h"
#endif
#ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#define SKIP_DEFORMABLE_BODY 1
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftBodySolvers.h"
@ -107,18 +106,16 @@
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.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
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#endif//SKIP_DEFORMABLE_BODY
#endif //SKIP_DEFORMABLE_BODY
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gVRTrackingObjectUniqueId = -1;
@ -1617,12 +1614,9 @@ struct PhysicsServerCommandProcessorInternalData
btScalar getDeltaTimeSubStep() const
{
btScalar deltaTimeSubStep = m_numSimulationSubSteps > 0 ?
m_physicsDeltaTime / m_numSimulationSubSteps :
m_physicsDeltaTime;
btScalar deltaTimeSubStep = m_numSimulationSubSteps > 0 ? m_physicsDeltaTime / m_numSimulationSubSteps : m_physicsDeltaTime;
return deltaTimeSubStep;
}
btScalar m_simulationTimestamp;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
@ -1653,15 +1647,14 @@ struct PhysicsServerCommandProcessorInternalData
btDefaultCollisionConfiguration* m_collisionConfiguration;
#ifndef SKIP_DEFORMABLE_BODY
btSoftBody* m_pickedSoftBody;
btDeformableMousePickingForce* m_mouseForce;
btScalar m_maxPickingForce;
btDeformableBodySolver* m_deformablebodySolver;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#endif
btMultiBodyDynamicsWorld* m_dynamicsWorld;
int m_constraintSolverType;
@ -1687,7 +1680,7 @@ struct PhysicsServerCommandProcessorInternalData
//data for picking objects
class btRigidBody* m_pickedBody;
int m_savedActivationState;
class btTypedConstraint* m_pickedConstraint;
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
@ -2631,9 +2624,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld()
{
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;
}
@ -2641,9 +2634,9 @@ btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformable
btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld()
{
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;
}
@ -2669,43 +2662,44 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
//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);
} else
}
else
{
btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
}
if (flags & RESET_USE_DEFORMABLE_WORLD)
if (flags & RESET_USE_DEFORMABLE_WORLD)
{
#ifndef SKIP_DEFORMABLE_BODY
m_data->m_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
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_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
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);
#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
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
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
}
if (0==m_data->m_dynamicsWorld)
if (0 == m_data->m_dynamicsWorld)
{
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);
}
//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);
@ -2863,43 +2857,42 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
delete mb;
}
#ifndef SKIP_DEFORMABLE_BODY
for (int j = 0; j < m_data->m_lf.size(); j++)
{
btDeformableLagrangianForce* force = m_data->m_lf[j];
delete force;
}
m_data->m_lf.clear();
for (int j = 0; j < m_data->m_lf.size(); j++)
{
btDeformableLagrangianForce* force = m_data->m_lf[j];
delete force;
}
m_data->m_lf.clear();
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
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);
delete sb;
}
}
}
#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
{
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
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);
delete sb;
}
}
}
#endif
}
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);
if (!body)
{
@ -3068,13 +3062,16 @@ int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex,
return userDataHandle;
}
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) {
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)
{
const std::string key = user_data_entries.getKeyAtIndex(i).m_string1;
const std::string* value = user_data_entries.getAtIndex(i);
if (value) {
if (value)
{
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
// data to the correct link in the MultiBody.
btHashMap<btHashString, int> linkNameToIndexMap;
if (bodyHandle->m_multiBody) {
if (bodyHandle->m_multiBody)
{
btMultiBody* mb = bodyHandle->m_multiBody;
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);
}
}
const UrdfModel* urdfModel = u2b.getUrdfModel();
if (urdfModel) {
if (urdfModel)
{
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);
int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
if (linkIndex) {
if (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);
}
}
@ -3522,7 +3525,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (loadOk)
{
btTransform rootTrans;
rootTrans.setOrigin(pos);
rootTrans.setRotation(orn);
@ -5095,19 +5097,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
btVector3 pt;
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 + 2]);
attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]);
convexHull->addPoint(pt * meshScale, false);
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 + 2]);
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
convexHull->addPoint(pt * meshScale, false);
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 + 2]);
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
convexHull->addPoint(pt * meshScale, false);
}
@ -5303,7 +5305,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
if (bodyHandle->m_multiBody)
{
//collision shape
if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1)
{
@ -5325,22 +5326,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
btTransform tr;
tr.setIdentity();
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;
}
gatherVertices(tr, colShape, vertices, collisionShapeIndex);
int numVertices = vertices.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
if (verticesCopied > 0)
{
memcpy(verticesOut, &vertices[0], sizeof(btVector3) * verticesCopied);
}
sizeInBytes = verticesCopied * sizeof(btVector3);
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
@ -5353,13 +5354,13 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
if (bodyHandle->m_softBody)
{
btSoftBody* psb = bodyHandle->m_softBody;
bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
for (int i = 0; i < verticesCopied; ++i)
{
if (separateRenderMesh)
@ -5818,7 +5819,6 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
{
m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
@ -5974,14 +5974,11 @@ struct CastSyncInfo
};
#endif // __cplusplus >= 201103L
struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask)
: 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)
: btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld),
m_collisionFilterMask(collisionFilterMask),
m_fractionEpsilon(fractionEpsilon)
m_collisionFilterMask(collisionFilterMask),
m_fractionEpsilon(fractionEpsilon)
{
}
@ -6027,7 +6024,7 @@ struct FilteredAllHitsRayResultCallback: public btCollisionWorld::AllHitsRayResu
{
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))
{
isDuplicate = true;
@ -6055,9 +6052,7 @@ struct BatchRayCaster
btScalar m_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_collisionFilterMask(collisionFilterMask),
m_fractionEpsilon(fractionEpsilon)
: 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_syncInfo = new CastSyncInfo;
}
@ -6162,7 +6157,7 @@ struct BatchRayCaster
{
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
}
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (body)
{
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
@ -6399,7 +6394,7 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
int usz = m_data->m_userConstraints.size();
int* constraintUid = bodyUids + actualNumBodies;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
for (int i = 0; i < usz; i++)
{
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
@ -6424,14 +6419,16 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
}
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];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (!body)
{
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]);
}
}
@ -6446,7 +6443,7 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
serverStatusOut.m_numDataStreamBytes = sizeInBytes;
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
return hasStatus;
}
@ -6496,7 +6493,8 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
addUserDataArgs.m_visualShapeIndex, addUserDataArgs.m_key,
bufferServerToClient, addUserDataArgs.m_valueLength,
addUserDataArgs.m_valueType);
if (userDataHandle < 0) {
if (userDataHandle < 0)
{
return hasStatus;
}
@ -7345,7 +7343,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
}
} //fi
//break;
//break;
}
}
} //if (body && body->m_rigidBody)
@ -7659,7 +7657,6 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
@ -7676,7 +7673,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3];
btVector3 center_of_mass(sb->getCenterOfMass());
btVector3 center_of_mass(sb->getCenterOfMass());
btTransform tr = sb->getRigidTransform();
//base position in world space, cartesian
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_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j];
}
else
{
@ -8088,9 +8085,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
cb.m_bodyUniqueIdA = bodyUniqueIdA;
cb.m_bodyUniqueIdB = bodyUniqueIdB;
cb.m_deltaTime = m_data->m_numSimulationSubSteps>0 ?
m_data->m_physicsDeltaTime/ m_data->m_numSimulationSubSteps :
m_data->m_physicsDeltaTime;
cb.m_deltaTime = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
for (int i = 0; i < setA.size(); i++)
{
@ -8441,7 +8436,6 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
#endif
}
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
@ -8761,7 +8755,6 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
return true;
}
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
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);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
}
#endif
return hasStatus;
}
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;
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;
if (linkIndex == -1)
{
@ -9695,7 +9686,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
{
SharedMemoryStatus& serverCmd = serverStatusOut;
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;
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;
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
else if (body && body->m_softBody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;
@ -9842,9 +9834,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
gforce->m_gravity = grav;
}
}
}
#endif
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;
}
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;
}
@ -9922,7 +9912,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
}
};
if (newSolver)
{
delete oldSolver;
@ -9987,23 +9977,23 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
#ifndef SKIP_DEFORMABLE_BODY
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
deformWorld ->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
deformWorld ->getWorldInfo().m_sparsesdf.Reset();
}
}
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
deformWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
deformWorld->getWorldInfo().m_sparsesdf.Reset();
}
}
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
softWorld->getWorldInfo().m_sparsesdf.Reset();
}
}
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
softWorld->getWorldInfo().m_sparsesdf.Reset();
}
}
#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 hasStatus = true;
BT_PROFILE("CMD_INIT_POSE");
if (m_data->m_verboseOutput)
@ -10089,7 +10079,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
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]);
mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling);
//refresh broadphase
m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(
@ -10272,7 +10262,7 @@ bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct S
bool hasStatus = true;
BT_PROFILE("CMD_RESET_SIMULATION");
m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0);
resetSimulation(clientCmd.m_updateFlags);
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)
{
btSoftBody* sb = body->m_softBody;
@ -11025,8 +11016,8 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 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 + 2]);
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 positionLocal(
clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
@ -11040,6 +11031,8 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
}
}
}
#endif
}
SharedMemoryStatus& serverCmd = serverStatusOut;
@ -11174,7 +11167,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
{
deformWorld->removeSoftBody(psb);
}
int graphicsInstance = psb->getUserIndex2();
m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
delete psb;
@ -11268,35 +11261,36 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
if (sbodyHandle->m_softBody)
{
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;
if (bodyUniqueId<=0)
if (bodyUniqueId <= 0)
{
//fixed anchor (mass = 0)
InteralUserConstraintData userConstraintData;
userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex;
userConstraintData.m_sbNodeIndex = 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++;
m_data->m_userConstraints.insert(uid, userConstraintData);
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} else
}
else
{
InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (mbodyHandle && mbodyHandle->m_multiBody)
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex;
if (linkIndex<0)
if (linkIndex < 0)
{
sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider());
} else
}
else
{
if (linkIndex < mbodyHandle->m_multiBody->getNumLinks())
{
@ -11322,11 +11316,11 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
{
bool disableCollisionBetweenLinkedBodies = true;
btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]);
clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]);
sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies);
}
}
#endif
}
int uid = m_data->m_userConstraintUIDGenerator++;
@ -11336,12 +11330,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
userConstraintData.m_sbNodeIndex = nodeIndex;
m_data->m_userConstraints.insert(uid, userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
}
}
#endif
@ -11690,9 +11680,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{
btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ?
m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps :
m_data->m_physicsDeltaTime;
btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
@ -11797,6 +11785,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
delete userConstraintPtr->m_rbConstraint;
m_data->m_userConstraints.remove(userConstraintUidRemove);
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (userConstraintPtr->m_sbHandle >= 0)
{
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_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
}
@ -13753,49 +13744,47 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),deformWorld->getDrawFlags());
btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
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();
if (softWorld)
{
for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++)
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i];
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++)
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),softWorld->getDrawFlags());
btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i];
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
}
}
}
struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
int m_faceId;
MyResultCallback(const btVector3& rayFromWorld, const btVector3& 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)
{
if (m_data->m_dynamicsWorld == 0)
@ -13994,7 +13982,6 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_pickingMultiBodyPoint2Point = 0;
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
//deformable/soft body?
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
@ -14006,7 +13993,6 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_pickedSoftBody = 0;
}
#endif
}
void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName)
@ -14273,7 +14259,7 @@ void PhysicsServerCommandProcessor::resetSimulation(int flags)
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
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;
void resetSimulation(int flags=0);
void resetSimulation(int flags = 0);
void createThreadPool();
class btDeformableMultiBodyDynamicsWorld* getDeformableWorld();
@ -120,7 +120,7 @@ public:
void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld(int flags=0);
virtual void createEmptyDynamicsWorld(int flags = 0);
virtual void deleteDynamicsWorld();
virtual bool connect()

9
src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h
View File

@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
btDeformableMultiBodyConstraintSolver* m_deformableSolver;
DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
btDispatcher* dispatcher)
: 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** 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());
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_islandAnalyticsData.push_back(m_solver->m_analyticsData);

144
src/BulletSoftBody/btCGProjection.h
View File

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

170
src/BulletSoftBody/btConjugateGradient.h
View File

@ -19,89 +19,93 @@
template <class MatrixX>
class btConjugateGradient : public btKrylovSolver<MatrixX>
{
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base;
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;
}
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base;
TVStack r, p, z, temp;
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());
}
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;
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 */

171
src/BulletSoftBody/btConjugateResidual.h
View File

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

303
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

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

715
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -20,462 +20,455 @@
#include "LinearMath/btQuickprof.h"
static const int kMaxConjugateGradientIterations = 300;
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(kMaxConjugateGradientIterations)
, m_cr(kMaxConjugateGradientIterations)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
, m_lineSearch(false)
, m_useProjection(false)
: m_numNodes(0), 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()
{
delete m_objective;
delete m_objective;
}
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
BT_PROFILE("solveDeformableConstraints");
if (!m_implicit)
{
m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual);
if (m_useProjection)
{
computeStep(m_dv, m_residual);
}
else
{
TVStack rhs, x;
m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
m_objective->addLagrangeMultiplier(m_dv, x);
m_objective->m_preconditioner->reinitialize(true);
computeStep(x, rhs);
for (int i = 0; i<m_dv.size(); ++i)
{
m_dv[i] = x[i];
}
}
updateVelocity();
}
else
{
for (int i = 0; i < m_maxNewtonIterations; ++i)
{
updateState();
// add the inertia term in the residual
int counter = 0;
for (int k = 0; k < m_softBodies.size(); ++k)
{
btSoftBody* psb = m_softBodies[k];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im > 0)
{
m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter];
}
++counter;
}
}
BT_PROFILE("solveDeformableConstraints");
if (!m_implicit)
{
m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual);
if (m_useProjection)
{
computeStep(m_dv, m_residual);
}
else
{
TVStack rhs, x;
m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
m_objective->addLagrangeMultiplier(m_dv, x);
m_objective->m_preconditioner->reinitialize(true);
computeStep(x, rhs);
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i] = x[i];
}
}
updateVelocity();
}
else
{
for (int i = 0; i < m_maxNewtonIterations; ++i)
{
updateState();
// add the inertia term in the residual
int counter = 0;
for (int k = 0; k < m_softBodies.size(); ++k)
{
btSoftBody* psb = m_softBodies[k];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im > 0)
{
m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
}
++counter;
}
}
m_objective->computeResidual(solverdt, m_residual);
if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
{
break;
}
// todo xuchenhan@: this really only needs to be calculated once
m_objective->applyDynamicFriction(m_residual);
if (m_lineSearch)
{
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 scale = 2;
btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2;
backupDv();
do {
scale *= beta;
if (scale < 1e-8) {
return;
}
updateEnergy(scale);
f1 = m_objective->totalEnergy(solverdt)+kineticEnergy();
f2 = f0 - alpha * scale * inner_product;
} while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues
revertDv();
updateDv(scale);
}
else
{
computeStep(m_ddv, m_residual);
updateDv();
}
for (int j = 0; j < m_numNodes; ++j)
{
m_ddv[j].setZero();
m_residual[j].setZero();
}
}
updateVelocity();
}
m_objective->computeResidual(solverdt, m_residual);
if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
{
break;
}
// todo xuchenhan@: this really only needs to be calculated once
m_objective->applyDynamicFriction(m_residual);
if (m_lineSearch)
{
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 scale = 2;
btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
backupDv();
do
{
scale *= beta;
if (scale < 1e-8)
{
return;
}
updateEnergy(scale);
f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
f2 = f0 - alpha * scale * inner_product;
} while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues
revertDv();
updateDv(scale);
}
else
{
computeStep(m_ddv, m_residual);
updateDv();
}
for (int j = 0; j < m_numNodes; ++j)
{
m_ddv[j].setZero();
m_residual[j].setZero();
}
}
updateVelocity();
}
}
btScalar btDeformableBodySolver::kineticEnergy()
{
btScalar ke = 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)
{
btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0)
{
ke += m_dv[node.index].length2() * 0.5 / node.m_im;
}
}
}
return ke;
btScalar ke = 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)
{
btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0)
{
ke += m_dv[node.index].length2() * 0.5 / node.m_im;
}
}
}
return ke;
}
void btDeformableBodySolver::backupDv()
{
m_backup_dv.resize(m_dv.size());
for (int i = 0; i<m_backup_dv.size(); ++i)
{
m_backup_dv[i] = m_dv[i];
}
m_backup_dv.resize(m_dv.size());
for (int i = 0; i < m_backup_dv.size(); ++i)
{
m_backup_dv[i] = m_dv[i];
}
}
void btDeformableBodySolver::revertDv()
{
for (int i = 0; i<m_backup_dv.size(); ++i)
{
m_dv[i] = m_backup_dv[i];
}
for (int i = 0; i < m_backup_dv.size(); ++i)
{
m_dv[i] = m_backup_dv[i];
}
}
void btDeformableBodySolver::updateEnergy(btScalar scale)
{
for (int i = 0; i<m_dv.size(); ++i)
{
m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
}
updateState();
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
}
updateState();
}
btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
{
m_cg.solve(*m_objective, ddv, residual, false);
btScalar inner_product = m_cg.dot(residual, m_ddv);
btScalar res_norm = m_objective->computeNorm(residual);
btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
if (inner_product < -tol)
{
if (verbose)
{
std::cout << "Looking backwards!" << std::endl;
}
for (int i = 0; i < m_ddv.size();++i)
{
m_ddv[i] = -m_ddv[i];
}
inner_product = -inner_product;
}
else if (std::abs(inner_product) < tol)
{
if (verbose)
{
std::cout << "Gradient Descent!" << std::endl;
}
btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
for (int i = 0; i < m_ddv.size();++i)
{
m_ddv[i] = scale * residual[i];
}
inner_product = scale * res_norm * res_norm;
}
return inner_product;
m_cg.solve(*m_objective, ddv, residual, false);
btScalar inner_product = m_cg.dot(residual, m_ddv);
btScalar res_norm = m_objective->computeNorm(residual);
btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
if (inner_product < -tol)
{
if (verbose)
{
std::cout << "Looking backwards!" << std::endl;
}
for (int i = 0; i < m_ddv.size(); ++i)
{
m_ddv[i] = -m_ddv[i];
}
inner_product = -inner_product;
}
else if (std::abs(inner_product) < tol)
{
if (verbose)
{
std::cout << "Gradient Descent!" << std::endl;
}
btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
for (int i = 0; i < m_ddv.size(); ++i)
{
m_ddv[i] = scale * residual[i];
}
inner_product = scale * res_norm * res_norm;
}
return inner_product;
}
void btDeformableBodySolver::updateState()
{
updateVelocity();
updateTempPosition();
updateVelocity();
updateTempPosition();
}
void btDeformableBodySolver::updateDv(btScalar scale)
{
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i] += scale * m_ddv[i];
}
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i] += scale * m_ddv[i];
}
}
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
if (m_useProjection)
m_cg.solve(*m_objective, ddv, residual, false);
else
m_cr.solve(*m_objective, ddv, residual, false);
if (m_useProjection)
m_cg.solve(*m_objective, ddv, residual, false);
else
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);
bool nodeUpdated = updateNodes();
if (nodeUpdated)
{
m_dv.resize(m_numNodes, btVector3(0,0,0));
m_ddv.resize(m_numNodes, btVector3(0,0,0));
m_residual.resize(m_numNodes, btVector3(0,0,0));
m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
}
// need to setZero here as resize only set value for newly allocated items
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i].setZero();
m_ddv[i].setZero();
m_residual[i].setZero();
}
m_dt = dt;
m_objective->reinitialize(nodeUpdated, dt);
updateSoftBodies();
m_softBodies.copyFromArray(softBodies);
bool nodeUpdated = updateNodes();
if (nodeUpdated)
{
m_dv.resize(m_numNodes, btVector3(0, 0, 0));
m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
m_residual.resize(m_numNodes, btVector3(0, 0, 0));
m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
}
// need to setZero here as resize only set value for newly allocated items
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i].setZero();
m_ddv[i].setZero();
m_residual[i].setZero();
}
m_dt = dt;
m_objective->reinitialize(nodeUpdated, dt);
updateSoftBodies();
}
void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("setConstraint");
m_objective->setConstraints(infoGlobal);
BT_PROFILE("setConstraint");
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");
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal);
return maxSquaredResidual;
BT_PROFILE("solveContactConstraints");
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
return maxSquaredResidual;
}
void btDeformableBodySolver::updateVelocity()
{
int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
psb->m_maxSpeedSquared = 0;
if (!psb->isActive())
{
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
// set NaN to zero;
if (m_dv[counter] != m_dv[counter])
{
m_dv[counter].setZero();
}
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter;
}
}
int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
psb->m_maxSpeedSquared = 0;
if (!psb->isActive())
{
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
// set NaN to zero;
if (m_dv[counter] != m_dv[counter])
{
m_dv[counter].setZero();
}
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter;
}
}
}
void btDeformableBodySolver::updateTempPosition()
{
int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
++counter;
}
psb->updateDeformation();
}
int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
++counter;
}
psb->updateDeformation();
}
}
void btDeformableBodySolver::backupVelocity()
{
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)
{
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
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)
{
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
}
void btDeformableBodySolver::setupDeformableSolve(bool implicit)
{
int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (implicit)
{
if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
else
m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
}
else
{
m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
}
psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter;
}
}
int counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (implicit)
{
if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
else
m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
}
else
{
m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
}
psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter;
}
}
}
void btDeformableBodySolver::revertVelocity()
{
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)
{
psb->m_nodes[j].m_v = m_backupVelocity[counter++];
}
}
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)
{
psb->m_nodes[j].m_v = m_backupVelocity[counter++];
}
}
}
bool btDeformableBodySolver::updateNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
numNodes += m_softBodies[i]->m_nodes.size();
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
return true;
}
return false;
int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
numNodes += m_softBodies[i]->m_nodes.size();
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
return true;
}
return false;
}
void btDeformableBodySolver::predictMotion(btScalar solverdt)
{
// apply explicit forces to velocity
m_objective->applyExplicitForce(m_residual);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody *psb = m_softBodies[i];
if (psb->isActive())
{
// predict motion for collision detection
predictDeformableMotion(psb, solverdt);
}
}
// apply explicit forces to velocity
m_objective->applyExplicitForce(m_residual);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (psb->isActive())
{
// predict motion for collision detection
predictDeformableMotion(psb, solverdt);
}
}
}
void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
{
BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
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;
}
BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
int i, ni;
/* 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);
/* 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 */
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()
{
BT_PROFILE("updateSoftBodies");
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody *psb = (btSoftBody *)m_softBodies[i];
if (psb->isActive())
{
psb->updateNormals();
}
}
BT_PROFILE("updateSoftBodies");
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody* psb = (btSoftBody*)m_softBodies[i];
if (psb->isActive())
{
psb->updateNormals();
}
}
}
void btDeformableBodySolver::setImplicit(bool implicit)
{
m_implicit = implicit;
m_objective->setImplicit(implicit);
m_implicit = implicit;
m_objective->setImplicit(implicit);
}
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
#define BT_DEFORMABLE_BODY_SOLVERS_H
#include "btSoftBodySolvers.h"
#include "btDeformableBackwardEulerObjective.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
@ -30,130 +29,132 @@ class btDeformableMultiBodyDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
{
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btAlignedObjectArray<btVector3> TVStack;
protected:
int m_numNodes; // total number of deformable body nodes
TVStack m_dv; // v_{n+1} - v_n
TVStack m_backup_dv; // backed up dv
TVStack m_ddv; // incremental dv
TVStack m_residual; // rhs of the linear solve
btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
btScalar m_dt; // dt
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver
bool m_implicit; // use implicit scheme if true, explicit scheme if false
int m_maxNewtonIterations; // max number of newton iterations
btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance
bool m_lineSearch; // If true, use newton's method with line search under implicit scheme
int m_numNodes; // total number of deformable body nodes
TVStack m_dv; // v_{n+1} - v_n
TVStack m_backup_dv; // backed up dv
TVStack m_ddv; // incremental dv
TVStack m_residual; // rhs of the linear solve
btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
btScalar m_dt; // dt
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver
bool m_implicit; // use implicit scheme if true, explicit scheme if false
int m_maxNewtonIterations; // max number of newton iterations
btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance
bool m_lineSearch; // If true, use newton's method with line search under implicit scheme
public:
// handles data related to objective function
btDeformableBackwardEulerObjective* m_objective;
bool m_useProjection;
btDeformableBodySolver();
virtual ~btDeformableBodySolver();
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// handles data related to objective function
btDeformableBackwardEulerObjective* m_objective;
bool m_useProjection;
// update soft body normals
virtual void updateSoftBodies();
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
btDeformableBodySolver();
// resize/clear data structures
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 ~btDeformableBodySolver();
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// 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);
}
// update soft body normals
virtual void updateSoftBodies();
// 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) {}
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// resize/clear data structures
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) {}
// 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 */

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

749
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

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

101
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -27,31 +27,30 @@
struct LagrangeMultiplier
{
int m_num_constraints; // Number of 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
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_num_constraints; // Number of 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
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;
};
class btDeformableContactProjection
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody*>& m_softBodies;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
#ifndef USE_MGS
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
#else
btAlignedObjectArray<btReducedVector> m_projections;
btAlignedObjectArray<btReducedVector> m_projections;
#endif
btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
// map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
// map from node index to node rigid constraint
@ -62,39 +61,39 @@ public:
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
// map from node index to node anchor constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
bool m_useStrainLimiting;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
{
}
virtual ~btDeformableContactProjection()
{
}
// apply the constraints to the rhs of the linear solve
virtual void project(TVStack& x);
// add friction force to the rhs of the linear solve
virtual void applyDynamicFriction(TVStack& f);
// update and solve the constraints
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.
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
// Set up projections for each vertex by adding the projection direction to
virtual void setProjection();
virtual void reinitialize(bool nodeUpdated);
btScalar solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
virtual void setLagrangeMultiplier();
void checkConstraints(const TVStack& x);
bool m_useStrainLimiting;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
: m_softBodies(softBodies)
{
}
virtual ~btDeformableContactProjection()
{
}
// apply the constraints to the rhs of the linear solve
virtual void project(TVStack& x);
// add friction force to the rhs of the linear solve
virtual void applyDynamicFriction(TVStack& f);
// update and solve the constraints
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.
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
// Set up projections for each vertex by adding the projection direction to
virtual void setProjection();
virtual void reinitialize(bool nodeUpdated);
btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
virtual void setLagrangeMultiplier();
void checkConstraints(const TVStack& x);
};
#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 const btPolarDecomposition polar;
return polar.decompose(m, q, s);
static const btPolarDecomposition polar;
return polar.decompose(m, q, s);
}
class btDeformableCorotatedForce : public btDeformableLagrangianForce
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
btDeformableCorotatedForce(): m_mu(1), m_lambda(1)
{
}
btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
{
}
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
}
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];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btMatrix3x3 P;
firstPiola(tetra.m_F,P);
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();
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
// explicit 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);
}
}
}
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
{
// btMatrix3x3 JFinvT = F.adjoint();
btScalar J = F.determinant();
P = F.adjoint().transpose() * (m_lambda * (J-1));
if (m_mu > SIMD_EPSILON)
{
btMatrix3x3 R,S;
if (J < 1024 * SIMD_EPSILON)
R.setIdentity();
else
PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
P += (F-R) * 2 * m_mu;
}
}
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 btDeformableLagrangianForceType getForceType()
{
return BT_COROTATED_FORCE;
}
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
{
}
btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
{
}
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
}
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];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btMatrix3x3 P;
firstPiola(tetra.m_F, P);
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();
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
// explicit 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);
}
}
}
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
{
// btMatrix3x3 JFinvT = F.adjoint();
btScalar J = F.determinant();
P = F.adjoint().transpose() * (m_lambda * (J - 1));
if (m_mu > SIMD_EPSILON)
{
btMatrix3x3 R, S;
if (J < 1024 * SIMD_EPSILON)
R.setIdentity();
else
PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
P += (F - R) * 2 * m_mu;
}
}
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 btDeformableLagrangianForceType getForceType()
{
return BT_COROTATED_FORCE;
}
};
#endif /* btCorotated_h */

160
src/BulletSoftBody/btDeformableGravityForce.h
View File

@ -21,87 +21,85 @@
class btDeformableGravityForce : public btDeformableLagrangianForce
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
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;
}
typedef btAlignedObjectArray<btVector3> TVStack;
btVector3 m_gravity;
// 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;
}
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
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 */

669
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@ -22,352 +22,349 @@
enum btDeformableLagrangianForceType
{
BT_GRAVITY_FORCE = 1,
BT_MASSSPRING_FORCE = 2,
BT_COROTATED_FORCE = 3,
BT_NEOHOOKEAN_FORCE = 4,
BT_LINEAR_ELASTICITY_FORCE = 5,
BT_MOUSE_PICKING_FORCE = 6
BT_GRAVITY_FORCE = 1,
BT_MASSSPRING_FORCE = 2,
BT_COROTATED_FORCE = 3,
BT_NEOHOOKEAN_FORCE = 4,
BT_LINEAR_ELASTICITY_FORCE = 5,
BT_MOUSE_PICKING_FORCE = 6
};
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
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *> m_softBodies;
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]);
}
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody*> m_softBodies;
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
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();
}
btDeformableLagrangianForce()
{
}
//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);
}
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)
{
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 */

625
src/BulletSoftBody/btDeformableLinearElasticityForce.h
View File

@ -21,320 +21,319 @@
class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
btScalar m_mu_damp, m_lambda_damp;
btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1)
{
btScalar damping = 0.05;
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(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
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * df_on_node0;
force[id1] -= scale1 * df_on_node123.getColumn(0);
force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 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_tetraScratches.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
energy += tetra.m_element_measure * elasticEnergyDensity(s);
}
}
return energy;
}
// 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)
{
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;
}
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
{
double density = 0;
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
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_lambda * trace * trace * 0.5;
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);
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
btScalar m_mu_damp, m_lambda_damp;
btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1)
{
btScalar damping = 0.05;
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(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
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * df_on_node0;
force[id1] -= scale1 * df_on_node123.getColumn(0);
force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 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_tetraScratches.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
energy += tetra.m_element_measure * elasticEnergyDensity(s);
}
}
return energy;
}
// 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)
{
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;
}
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
{
double density = 0;
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
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_lambda * trace * trace * 0.5;
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);
#if USE_SVD
if (max_p > 0)
{
// 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
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
if (trPTP > max_p * max_p)
{
btMatrix3x3 U, V;
btVector3 sigma;
singularValueDecomposition(P, U, sigma, V);
sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p);
btMatrix3x3 Sigma;
Sigma.setIdentity();
Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose();
}
}
if (max_p > 0)
{
// 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
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
if (trPTP > max_p * max_p)
{
btMatrix3x3 U, V;
btVector3 sigma;
singularValueDecomposition(P, U, sigma, V);
sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p);
btMatrix3x3 Sigma;
Sigma.setIdentity();
Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose();
}
}
#endif
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
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
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
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, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDifferential(psb->m_tetraScratches[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;
// elastic force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
{
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
}
// 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 trace = (dF[0][0] + dF[1][1] + dF[2][2]);
dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
}
// 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 trace = (dF[0][0] + dF[1][1] + dF[2][2]);
dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace;
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_LINEAR_ELASTICITY_FORCE;
}
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
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
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
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, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDifferential(psb->m_tetraScratches[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;
// elastic force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
{
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
}
// 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 trace = (dF[0][0] + dF[1][1] + dF[2][2]);
dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
}
// 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 trace = (dF[0][0] + dF[1][1] + dF[2][2]);
dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace;
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_LINEAR_ELASTICITY_FORCE;
}
};
#endif /* BT_LINEAR_ELASTICITY_H */

544
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@ -20,282 +20,282 @@
class btDeformableMassSpringForce : public btDeformableLagrangianForce
{
// 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
bool m_momentum_conserving;
btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
// 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
bool m_momentum_conserving;
btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
public:
typedef btAlignedObjectArray<btVector3> TVStack;
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)
{
if (m_bendingStiffness < btScalar(0))
{
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;
typedef btAlignedObjectArray<btVector3> TVStack;
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)
{
if (m_bendingStiffness < btScalar(0))
{
m_bendingStiffness = m_elasticStiffness;
}
}
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;
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
// 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;
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
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;
}
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]);
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 */

238
src/BulletSoftBody/btDeformableMousePickingForce.h
View File

@ -20,126 +20,126 @@
class btDeformableMousePickingForce : public btDeformableLagrangianForce
{
// 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
btScalar m_elasticStiffness, m_dampingStiffness;
const btSoftBody::Face& m_face;
btVector3 m_mouse_pos;
btScalar m_maxForce;
// 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
btScalar m_elasticStiffness, m_dampingStiffness;
const btSoftBody::Face& m_face;
btVector3 m_mouse_pos;
btScalar m_maxForce;
public:
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)
{
}
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)
{
for (int i = 0; i < 3; ++i)
{
btVector3 v_diff = m_face.m_n[i]->m_v;
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
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();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
}
force[m_face.m_n[i]->index] -= scaled_force;
}
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
btScalar scaled_stiffness = scale * m_elasticStiffness;
for (int i = 0; i < 3; ++i)
{
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
btVector3 scaled_force = scaled_stiffness * dir;
if (scaled_force.safeNorm() > m_maxForce)
{
scaled_force.safeNormalize();
scaled_force *= m_maxForce;
}
force[m_face.m_n[i]->index] -= scaled_force;
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int i = 0; i < 3; ++i)
{
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)
{
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;
}
df[m_face.m_n[i]->index] -= local_scaled_df;
}
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 0;
for (int i = 0; i < 3; ++i)
{
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
btVector3 scaled_force = m_elasticStiffness * dir;
if (scaled_force.safeNorm() > m_maxForce)
{
scaled_force.safeNormalize();
scaled_force *= m_maxForce;
}
energy += 0.5 * scaled_force.dot(dir);
}
return energy;
}
virtual double totalDampingEnergy(btScalar dt)
{
double energy = 0;
for (int i = 0; i < 3; ++i)
{
btVector3 v_diff = m_face.m_n[i]->m_v;
btVector3 scaled_force = m_dampingStiffness * v_diff;
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();
scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
}
energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
}
return energy;
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
//TODO
}
void setMousePos(const btVector3& p)
{
m_mouse_pos = p;
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_MOUSE_PICKING_FORCE;
}
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)
{
}
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)
{
for (int i = 0; i < 3; ++i)
{
btVector3 v_diff = m_face.m_n[i]->m_v;
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
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();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
}
force[m_face.m_n[i]->index] -= scaled_force;
}
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
btScalar scaled_stiffness = scale * m_elasticStiffness;
for (int i = 0; i < 3; ++i)
{
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
btVector3 scaled_force = scaled_stiffness * dir;
if (scaled_force.safeNorm() > m_maxForce)
{
scaled_force.safeNormalize();
scaled_force *= m_maxForce;
}
force[m_face.m_n[i]->index] -= scaled_force;
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int i = 0; i < 3; ++i)
{
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)
{
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;
}
df[m_face.m_n[i]->index] -= local_scaled_df;
}
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 0;
for (int i = 0; i < 3; ++i)
{
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
btVector3 scaled_force = m_elasticStiffness * dir;
if (scaled_force.safeNorm() > m_maxForce)
{
scaled_force.safeNormalize();
scaled_force *= m_maxForce;
}
energy += 0.5 * scaled_force.dot(dir);
}
return energy;
}
virtual double totalDampingEnergy(btScalar dt)
{
double energy = 0;
for (int i = 0; i < 3; ++i)
{
btVector3 v_diff = m_face.m_n[i]->m_v;
btVector3 scaled_force = m_dampingStiffness * v_diff;
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();
scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
}
energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
}
return energy;
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
//TODO
}
void setMousePos(const btVector3& p)
{
m_mouse_pos = p;
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_MOUSE_PICKING_FORCE;
}
};
#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.
*/
#include "btDeformableMultiBodyConstraintSolver.h"
#include <iostream>
// override the iterations method to include deformable/multibody contact
btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
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)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
// 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
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// solver body velocity -> rigid body velocity
solverBodyWriteBack(infoGlobal);
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
// update rigid body velocity in rigid/deformable contact
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
// solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal);
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
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
// solve rigid/rigid in solver body
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// solver body velocity -> rigid body velocity
solverBodyWriteBack(infoGlobal);
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
// update rigid body velocity in rigid/deformable contact
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
// solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal);
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
if (iteration >= (maxIterations - 1))
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
if (iteration >= (maxIterations - 1))
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
m_analyticsData.m_numSolverCalls++;
m_analyticsData.m_numIterationsUsed = iteration+1;
m_analyticsData.m_islandId = -2;
if (numBodies>0)
m_analyticsData.m_islandId = bodies[0]->getCompanionId();
m_analyticsData.m_numBodies = numBodies;
m_analyticsData.m_numContactManifolds = numManifolds;
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
break;
}
}
}
return 0.f;
m_analyticsData.m_numSolverCalls++;
m_analyticsData.m_numIterationsUsed = iteration + 1;
m_analyticsData.m_islandId = -2;
if (numBodies > 0)
m_analyticsData.m_islandId = bodies[0]->getCompanionId();
m_analyticsData.m_numBodies = numBodies;
m_analyticsData.m_numContactManifolds = numManifolds;
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
break;
}
}
}
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_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
// inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// overriden
solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
m_tmpMultiBodyConstraints = 0;
m_tmpNumMultiBodyConstraints = 0;
m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
// inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// overriden
solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
m_tmpMultiBodyConstraints = 0;
m_tmpNumMultiBodyConstraints = 0;
}
void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{
for (int i = 0; i < numBodies; i++)
{
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
for (int i = 0; i < numBodies; i++)
{
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass())
{
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
}
}
btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass())
{
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
}
}
}
void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
{
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
{
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
if (body)
{
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);
}
}
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
{
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
if (body)
{
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);
}
}
}
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");
int iteration;
if (infoGlobal.m_splitImpulse)
{
{
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
btScalar leastSquaresResidual = 0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j = 0; j < numPoolConstraints; j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
// solve the position correction between deformable and rigid/multibody
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
int iteration;
if (infoGlobal.m_splitImpulse)
{
{
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
btScalar leastSquaresResidual = 0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j = 0; j < numPoolConstraints; j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
// solve the position correction between deformable and rigid/multibody
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
if (iteration >= (infoGlobal.m_numIterations - 1))
printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
if (iteration >= (infoGlobal.m_numIterations - 1))
printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
break;
}
}
}
}
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.
*/
#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
@ -32,30 +31,31 @@ class btDeformableBodySolver;
ATTRIBUTE_ALIGNED16(class)
btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
{
btDeformableBodySolver* m_deformableSolver;
protected:
// 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);
// write the velocity of the the solver body to the underlying rigid body
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
btDeformableBodySolver* m_deformableSolver;
protected:
// 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);
// write the velocity of the the solver body to the underlying rigid body
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:
BT_DECLARE_ALIGNED_ALLOCATOR();
void setDeformableSolver(btDeformableBodySolver* 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);
BT_DECLARE_ALIGNED_ALLOCATOR();
void setDeformableSolver(btDeformableBodySolver * 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);
};
#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
{
typedef btAlignedObjectArray<btVector3> TVStack;
///Solver classes that encapsulate multiple deformable bodies for solving
btDeformableBodySolver* m_deformableBodySolver;
btSoftBodyArray m_softBodies;
int m_drawFlags;
bool m_drawNodeTree;
bool m_drawFaceTree;
bool m_drawClusterTree;
btSoftBodyWorldInfo m_sbi;
btScalar m_internalTime;
int m_ccdIterations;
bool m_implicit;
bool m_lineSearch;
bool m_useProjection;
DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback;
typedef btAlignedObjectArray<btVector3> TVStack;
///Solver classes that encapsulate multiple deformable bodies for solving
btDeformableBodySolver* m_deformableBodySolver;
btSoftBodyArray m_softBodies;
int m_drawFlags;
bool m_drawNodeTree;
bool m_drawFaceTree;
bool m_drawClusterTree;
btSoftBodyWorldInfo m_sbi;
btScalar m_internalTime;
int m_ccdIterations;
bool m_implicit;
bool m_lineSearch;
bool m_useProjection;
DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback;
protected:
virtual void internalSingleStepSimulation(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep);
void positionCorrection(btScalar timeStep);
void solveConstraints(btScalar timeStep);
void updateActivationState(btScalar timeStep);
void clearGravity();
virtual void internalSingleStepSimulation(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep);
void positionCorrection(btScalar timeStep);
void solveConstraints(btScalar timeStep);
void updateActivationState(btScalar timeStep);
void clearGravity();
public:
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();
void setSolverCallback(btSolverCallback cb)
{
m_solverCallback = cb;
}
virtual ~btDeformableMultiBodyDynamicsWorld();
virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
{
return (btMultiBodyDynamicsWorld*)(this);
}
virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
{
return (const btMultiBodyDynamicsWorld*)(this);
}
virtual btDynamicsWorldType getWorldType() const
{
return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
}
virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
btSoftBodyArray& getSoftBodyArray()
{
return m_softBodies;
}
const btSoftBodyArray& getSoftBodyArray() const
{
return m_softBodies;
}
btSoftBodyWorldInfo& getWorldInfo()
{
return m_sbi;
}
const btSoftBodyWorldInfo& getWorldInfo() const
{
return m_sbi;
}
void reinitialize(btScalar timeStep);
void applyRigidBodyGravity(btScalar timeStep);
void beforeSolverCallbacks(btScalar timeStep);
void afterSolverCallbacks(btScalar timeStep);
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeSoftBody(btSoftBody* body);
void removeCollisionObject(btCollisionObject* collisionObject);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }
void setupConstraints();
void performDeformableCollisionDetection();
void solveMultiBodyConstraints();
void solveContactConstraints();
void sortConstraints();
void softBodySelfCollision();
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
void setLineSearch(bool lineSearch)
{
m_lineSearch = lineSearch;
}
void applyRepulsionForce(btScalar timeStep);
void performGeometricCollisions(btScalar timeStep);
struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
{
btVector3 m_rayFromWorld;
btVector3 m_rayToWorld;
btTransform m_rayFromTrans;
btTransform m_rayToTrans;
btVector3 m_hitNormal;
const btDeformableMultiBodyDynamicsWorld* m_world;
btCollisionWorld::RayResultCallback& m_resultCallback;
btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
: m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld),
m_world(world),
m_resultCallback(resultCallback)
{
m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin(m_rayFromWorld);
m_rayToTrans.setIdentity();
m_rayToTrans.setOrigin(m_rayToWorld);
btVector3 rayDir = (rayToWorld - rayFromWorld);
rayDir.normalize();
///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[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_signs[0] = m_rayDirectionInverse[0] < 0.0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
}
virtual bool process(const btBroadphaseProxy* proxy)
{
///terminate further ray tests, once the closestHitFraction reached zero
if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
return false;
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
//only perform raycast if filterMask matches
if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
void setSolverCallback(btSolverCallback cb)
{
m_solverCallback = cb;
}
virtual ~btDeformableMultiBodyDynamicsWorld();
virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
{
return (btMultiBodyDynamicsWorld*)(this);
}
virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
{
return (const btMultiBodyDynamicsWorld*)(this);
}
virtual btDynamicsWorldType getWorldType() const
{
return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
}
virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
btSoftBodyArray& getSoftBodyArray()
{
return m_softBodies;
}
const btSoftBodyArray& getSoftBodyArray() const
{
return m_softBodies;
}
btSoftBodyWorldInfo& getWorldInfo()
{
return m_sbi;
}
const btSoftBodyWorldInfo& getWorldInfo() const
{
return m_sbi;
}
void reinitialize(btScalar timeStep);
void applyRigidBodyGravity(btScalar timeStep);
void beforeSolverCallbacks(btScalar timeStep);
void afterSolverCallbacks(btScalar timeStep);
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeSoftBody(btSoftBody* body);
void removeCollisionObject(btCollisionObject* collisionObject);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }
void setupConstraints();
void performDeformableCollisionDetection();
void solveMultiBodyConstraints();
void solveContactConstraints();
void sortConstraints();
void softBodySelfCollision();
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
void setLineSearch(bool lineSearch)
{
m_lineSearch = lineSearch;
}
void applyRepulsionForce(btScalar timeStep);
void performGeometricCollisions(btScalar timeStep);
struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
{
btVector3 m_rayFromWorld;
btVector3 m_rayToWorld;
btTransform m_rayFromTrans;
btTransform m_rayToTrans;
btVector3 m_hitNormal;
const btDeformableMultiBodyDynamicsWorld* m_world;
btCollisionWorld::RayResultCallback& m_resultCallback;
btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
: m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld),
m_world(world),
m_resultCallback(resultCallback)
{
m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin(m_rayFromWorld);
m_rayToTrans.setIdentity();
m_rayToTrans.setOrigin(m_rayToWorld);
btVector3 rayDir = (rayToWorld - rayFromWorld);
rayDir.normalize();
///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[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_signs[0] = m_rayDirectionInverse[0] < 0.0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
}
virtual bool process(const btBroadphaseProxy* proxy)
{
///terminate further ray tests, once the closestHitFraction reached zero
if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
return false;
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
//only perform raycast if filterMask matches
if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
#if 0
#ifdef RECALCULATE_AABB
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
@ -225,87 +225,85 @@ public:
const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
#endif
#endif
//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
//culling already done by broadphase
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
{
m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
m_resultCallback);
}
}
return true;
}
};
//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
//culling already done by broadphase
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
{
m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
m_resultCallback);
}
}
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
m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
#else
for (int i = 0; i < this->getNumCollisionObjects(); i++)
{
rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
}
for (int i = 0; i < this->getNumCollisionObjects(); i++)
{
rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback) const
{
if (collisionShape->isSoftBody())
{
btSoftBody* softBody = btSoftBody::upcast(collisionObject);
if (softBody)
{
btSoftBody::sRayCast softResult;
if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
{
if (softResult.fraction <= resultCallback.m_closestHitFraction)
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = 0;
shapeInfo.m_triangleIndex = softResult.index;
// get the normal
btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
btVector3 normal = -rayDir;
normal.normalize();
{
normal = softBody->m_faces[softResult.index].m_normal;
if (normal.dot(rayDir) > 0)
{
// normal always point toward origin of the ray
normal = -normal;
}
}
btCollisionWorld::LocalRayResult rayResult(collisionObject,
&shapeInfo,
normal,
softResult.fraction);
bool normalInWorldSpace = true;
resultCallback.addSingleResult(rayResult, normalInWorldSpace);
}
}
}
}
else
{
btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
}
}
}
void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback) const
{
if (collisionShape->isSoftBody())
{
btSoftBody* softBody = btSoftBody::upcast(collisionObject);
if (softBody)
{
btSoftBody::sRayCast softResult;
if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
{
if (softResult.fraction <= resultCallback.m_closestHitFraction)
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = 0;
shapeInfo.m_triangleIndex = softResult.index;
// get the normal
btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
btVector3 normal = -rayDir;
normal.normalize();
{
normal = softBody->m_faces[softResult.index].m_normal;
if (normal.dot(rayDir) > 0)
{
// normal always point toward origin of the ray
normal = -normal;
}
}
btCollisionWorld::LocalRayResult rayResult(collisionObject,
&shapeInfo,
normal,
softResult.fraction);
bool normalInWorldSpace = true;
resultCallback.addSingleResult(rayResult, normalInWorldSpace);
}
}
}
}
else
{
btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
}
}
};
#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
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda; // Lame Parameters
btScalar m_E, m_nu; // Young's modulus and Poisson ratio
btScalar m_mu_damp, m_lambda_damp;
btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
{
btScalar damping = 0.05;
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda; // Lame Parameters
btScalar m_E, m_nu; // Young's modulus and Poisson ratio
btScalar m_mu_damp, m_lambda_damp;
btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
{
btScalar damping = 0.05;
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
updateYoungsModulusAndPoissonRatio();
}
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
updateYoungsModulusAndPoissonRatio();
}
}
void updateYoungsModulusAndPoissonRatio()
{
// conversion from Lame Parameters to Young's modulus and Poisson ratio
// 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);
}
@ -55,21 +55,21 @@ public:
// conversion from Young's modulus and Poisson ratio to Lame Parameters
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
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;
updateLameParameters();
}
}
void setPoissonRatio(btScalar nu)
{
m_nu = nu;
updateLameParameters();
}
void setDamping(btScalar damping)
{
m_mu_damp = damping * m_mu;
@ -83,339 +83,338 @@ public:
updateYoungsModulusAndPoissonRatio();
}
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(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();
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
// damping force differential
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * df_on_node0;
force[id1] -= scale1 * df_on_node123.getColumn(0);
force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 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_tetraScratches.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
energy += tetra.m_element_measure * elasticEnergyDensity(s);
}
}
return energy;
}
// 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)
{
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;
}
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);
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
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * df_on_node0;
force[id1] -= scale1 * df_on_node123.getColumn(0);
force[id2] -= scale1 * df_on_node123.getColumn(1);
force[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual double totalElasticEnergy(btScalar dt)
{
double energy = 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_tetraScratches.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
energy += tetra.m_element_measure * elasticEnergyDensity(s);
}
}
return energy;
}
// 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)
{
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;
}
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
if (max_p > 0)
{
// 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
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
if (trPTP > max_p * max_p)
{
btMatrix3x3 U, V;
btVector3 sigma;
singularValueDecomposition(P, U, sigma, V);
sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p);
btMatrix3x3 Sigma;
Sigma.setIdentity();
Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose();
}
}
if (max_p > 0)
{
// 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
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
if (trPTP > max_p * max_p)
{
btMatrix3x3 U, V;
btVector3 sigma;
singularValueDecomposition(P, U, sigma, V);
sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p);
btMatrix3x3 Sigma;
Sigma.setIdentity();
Sigma[0][0] = sigma[0];
Sigma[1][1] = sigma[1];
Sigma[2][2] = sigma[2];
P = U * Sigma * V.transpose();
}
}
#endif
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
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;
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
// damping force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
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, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDifferential(psb->m_tetraScratches[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;
// elastic force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
{
btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
P = s.m_F * c1 + s.m_cofF * c2;
}
// 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;
}
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
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
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, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDifferential(psb->m_tetraScratches[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;
// elastic force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
{
btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
P = s.m_F * c1 + s.m_cofF * c2;
}
// 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 */

153
src/BulletSoftBody/btKrylovSolver.h
View File

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

471
src/BulletSoftBody/btPreconditioner.h
View File

@ -19,269 +19,266 @@
class Preconditioner
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
virtual void operator()(const TVStack& x, TVStack& b) = 0;
virtual void reinitialize(bool nodeUpdated) = 0;
virtual ~Preconditioner(){}
typedef btAlignedObjectArray<btVector3> TVStack;
virtual void operator()(const TVStack& x, TVStack& b) = 0;
virtual void reinitialize(bool nodeUpdated) = 0;
virtual ~Preconditioner() {}
};
class DefaultPreconditioner : public Preconditioner
{
public:
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i];
}
virtual void reinitialize(bool nodeUpdated)
{
}
virtual ~DefaultPreconditioner(){}
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i];
}
virtual void reinitialize(bool nodeUpdated)
{
}
virtual ~DefaultPreconditioner() {}
};
class MassPreconditioner : public Preconditioner
{
btAlignedObjectArray<btScalar> m_inv_mass;
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];
}
}
};
btAlignedObjectArray<btScalar> m_inv_mass;
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];
}
}
};
class KKTPreconditioner : public Preconditioner
{
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
const btDeformableContactProjection& m_projections;
const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
TVStack m_inv_A, m_inv_S;
const btScalar& m_dt;
const bool& m_implicit;
const btAlignedObjectArray<btSoftBody*>& m_softBodies;
const btDeformableContactProjection& m_projections;
const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
TVStack m_inv_A, m_inv_S;
const btScalar& m_dt;
const bool& m_implicit;
public:
KKTPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
: m_softBodies(softBodies)
, m_projections(projections)
, m_lf(lf)
, m_dt(dt)
, m_implicit(implicit)
{
}
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
int num_nodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
num_nodes += psb->m_nodes.size();
}
m_inv_A.resize(num_nodes);
}
buildDiagonalA(m_inv_A);
for (int i = 0; i < m_inv_A.size(); ++i)
{
// 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_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0/ m_inv_A[i][d];
}
}
m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
// 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)
{
// 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)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
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);
++counter;
}
}
if (m_implicit)
{
printf("implicit not implemented\n");
btAssert(false);
}
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)
{
// S[k,k] = e_k^T * C A_d^-1 C^T * e_k
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
btVector3& t = diagS[c];
t.setZero();
for (int j = 0; j < lm.m_num_constraints; ++j)
{
for (int i = 0; i < lm.m_num_nodes; ++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];
}
}
}
}
}
KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
: m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
{
}
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
int num_nodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
num_nodes += psb->m_nodes.size();
}
m_inv_A.resize(num_nodes);
}
buildDiagonalA(m_inv_A);
for (int i = 0; i < m_inv_A.size(); ++i)
{
// 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_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
}
}
m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
// 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)
{
// 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)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
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);
++counter;
}
}
if (m_implicit)
{
printf("implicit not implemented\n");
btAssert(false);
}
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)
{
// S[k,k] = e_k^T * C A_d^-1 C^T * e_k
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
btVector3& t = diagS[c];
t.setZero();
for (int j = 0; j < lm.m_num_constraints; ++j)
{
for (int i = 0; i < lm.m_num_nodes; ++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
#ifndef USE_FULL_PRECONDITIONER
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] = x[i] * m_inv_A[i];
}
int offset = m_inv_A.size();
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i+offset] = x[i+offset] * m_inv_S[i];
}
}
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] = x[i] * m_inv_A[i];
}
int offset = m_inv_A.size();
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i + offset] = x[i + offset] * m_inv_S[i];
}
}
#else
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
int offset = m_inv_A.size();
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
int offset = m_inv_A.size();
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] = x[i] * m_inv_A[i];
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] = x[i] * m_inv_A[i];
}
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i+offset].setZero();
}
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i + offset].setZero();
}
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
// C * x
for (int d = 0; d < lm.m_num_constraints; ++d)
{
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]);
}
}
}
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
// C * x
for (int d = 0; d < lm.m_num_constraints; ++d)
{
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]);
}
}
}
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i+offset] = b[i+offset] * m_inv_S[i];
}
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i + offset] = b[i + offset] * m_inv_S[i];
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i].setZero();
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i].setZero();
}
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
// C^T * lambda
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i)
{
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];
}
}
}
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
// C^T * lambda
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i)
{
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];
}
}
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] = (x[i] - b[i]) * m_inv_A[i];
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] = (x[i] - b[i]) * m_inv_A[i];
}
TVStack t;
t.resize(b.size());
for (int i = 0; i < m_inv_S.size(); ++i)
{
t[i+offset] = x[i+offset] * m_inv_S[i];
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
t[i].setZero();
}
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
// C^T * lambda
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i)
{
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];
}
}
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] += t[i] * m_inv_A[i];
}
TVStack t;
t.resize(b.size());
for (int i = 0; i < m_inv_S.size(); ++i)
{
t[i + offset] = x[i + offset] * m_inv_S[i];
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
t[i].setZero();
}
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
{
// C^T * lambda
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
for (int i = 0; i < lm.m_num_nodes; ++i)
{
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];
}
}
}
for (int i = 0; i < m_inv_A.size(); ++i)
{
b[i] += t[i] * m_inv_A[i];
}
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i+offset] -= x[i+offset] * m_inv_S[i];
}
}
for (int i = 0; i < m_inv_S.size(); ++i)
{
b[i + offset] -= x[i + offset] * m_inv_S[i];
}
}
#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
#define btSoftBodyData 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;
//#endif //BT_USE_DOUBLE_PRECISION
@ -171,10 +171,10 @@ public:
CL_SELF = 0x0040, ///Cluster soft body self collision
VF_DD = 0x0080, ///Vertex vs face soft vs soft handling
RVDFmask = 0x0f00, /// Rigid versus deformable face mask
SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face
SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face
SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node
RVDFmask = 0x0f00, /// Rigid versus deformable face mask
SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face
SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face
SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node
/* presets */
Default = SDF_RS,
END
@ -226,7 +226,7 @@ public:
const btCollisionObject* m_colObj; /* Rigid body */
btVector3 m_normal; /* Outward normal */
btScalar m_offset; /* Offset from origin */
btVector3 m_bary; /* Barycentric weights for faces */
btVector3 m_bary; /* Barycentric weights for faces */
};
/* sMedium */
@ -263,7 +263,7 @@ public:
btVector3 m_x; // Position
btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity
btVector3 m_vn; // Previous step velocity
btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal
btScalar m_im; // 1/mass
@ -291,37 +291,37 @@ public:
/* Face */
struct Face : Feature
{
Node* m_n[3]; // Node pointers
btVector3 m_normal; // Normal
btScalar m_ra; // Rest area
btDbvtNode* m_leaf; // Leaf data
btVector4 m_pcontact; // barycentric weights of the persistent contact
btVector3 m_n0, m_n1, m_vn;
Node* m_n[3]; // Node pointers
btVector3 m_normal; // Normal
btScalar m_ra; // Rest area
btDbvtNode* m_leaf; // Leaf data
btVector4 m_pcontact; // barycentric weights of the persistent contact
btVector3 m_n0, m_n1, m_vn;
int m_index;
};
/* Tetra */
struct Tetra : Feature
{
Node* m_n[4]; // Node pointers
btScalar m_rv; // Rest volume
btDbvtNode* m_leaf; // Leaf data
btVector3 m_c0[4]; // gradients
btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
btScalar m_c2; // m_c1/sum(|g0..3|^2)
btMatrix3x3 m_Dm_inverse; // rest Dm^-1
btMatrix3x3 m_F;
btScalar m_element_measure;
Node* m_n[4]; // Node pointers
btScalar m_rv; // Rest volume
btDbvtNode* m_leaf; // Leaf data
btVector3 m_c0[4]; // gradients
btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
btScalar m_c2; // m_c1/sum(|g0..3|^2)
btMatrix3x3 m_Dm_inverse; // rest Dm^-1
btMatrix3x3 m_F;
btScalar m_element_measure;
};
/* TetraScratch */
struct TetraScratch
{
btMatrix3x3 m_F; // deformation gradient F
btScalar m_trace; // trace of F^T * F
btScalar m_J; // det(F)
btMatrix3x3 m_cofF; // cofactor of F
};
/* TetraScratch */
struct TetraScratch
{
btMatrix3x3 m_F; // deformation gradient F
btScalar m_trace; // trace of F^T * F
btScalar m_J; // det(F)
btMatrix3x3 m_cofF; // cofactor of F
};
/* RContact */
struct RContact
{
@ -332,67 +332,67 @@ public:
btScalar m_c2; // ima*dt
btScalar m_c3; // Friction
btScalar m_c4; // Hardness
// jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2;
btVector3 t1;
btVector3 t2;
// jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2;
btVector3 t1;
btVector3 t2;
};
class DeformableRigidContact
{
public:
sCti m_cti; // Contact infos
btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor
btScalar m_c2; // inverse mass of node/face
btScalar m_c3; // Friction
btScalar m_c4; // Hardness
// jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2;
btVector3 t1;
btVector3 t2;
};
class DeformableNodeRigidContact : public DeformableRigidContact
{
public:
Node* m_node; // Owner node
};
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
{
public:
btVector3 m_local; // Anchor position in body space
};
class DeformableFaceRigidContact : public DeformableRigidContact
{
public:
Face* m_face; // Owner face
btVector3 m_contactPoint; // Contact point
btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
};
struct DeformableFaceNodeContact
{
Node* m_node; // Node
Face* m_face; // Face
btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
btVector3 m_normal; // Normal
btScalar m_margin; // Margin
btScalar m_friction; // Friction
btScalar m_imf; // inverse mass of the face at contact point
btScalar m_c0; // scale of the impulse matrix;
};
class DeformableRigidContact
{
public:
sCti m_cti; // Contact infos
btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor
btScalar m_c2; // inverse mass of node/face
btScalar m_c3; // Friction
btScalar m_c4; // Hardness
// jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_t1;
btMultiBodyJacobianData jacobianData_t2;
btVector3 t1;
btVector3 t2;
};
class DeformableNodeRigidContact : public DeformableRigidContact
{
public:
Node* m_node; // Owner node
};
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
{
public:
btVector3 m_local; // Anchor position in body space
};
class DeformableFaceRigidContact : public DeformableRigidContact
{
public:
Face* m_face; // Owner face
btVector3 m_contactPoint; // Contact point
btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
};
struct DeformableFaceNodeContact
{
Node* m_node; // Node
Face* m_face; // Face
btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
btVector3 m_normal; // Normal
btScalar m_margin; // Margin
btScalar m_friction; // Friction
btScalar m_imf; // inverse mass of the face at contact point
btScalar m_c0; // scale of the impulse matrix;
};
/* SContact */
struct SContact
{
@ -719,19 +719,19 @@ public:
tVSolverArray m_vsequence; // Velocity solvers sequence
tPSolverArray m_psequence; // Position solvers sequence
tPSolverArray m_dsequence; // Drift solvers sequence
btScalar drag; // deformable air drag
btScalar m_maxStress; // Maximum principle first Piola stress
btScalar drag; // deformable air drag
btScalar m_maxStress; // Maximum principle first Piola stress
};
/* SolverState */
struct SolverState
{
//if you add new variables, always initialize them!
SolverState()
:sdt(0),
isdt(0),
velmrg(0),
radmrg(0),
updmrg(0)
: sdt(0),
isdt(0),
velmrg(0),
radmrg(0),
updmrg(0)
{
}
btScalar sdt; // dt*timescale
@ -792,41 +792,41 @@ public:
btSoftBodyWorldInfo* m_worldInfo; // World info
tNoteArray m_notes; // Notes
tNodeArray m_nodes; // Nodes
tNodeArray m_renderNodes; // Nodes
tNodeArray m_renderNodes; // Nodes
tLinkArray m_links; // Links
tFaceArray m_faces; // Faces
tFaceArray m_renderFaces; // Faces
tFaceArray m_renderFaces; // Faces
tTetraArray m_tetras; // Tetras
btAlignedObjectArray<TetraScratch> m_tetraScratches;
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
tAnchorArray m_anchors; // Anchors
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
tSContactArray m_scontacts; // Soft contacts
tJointArray m_joints; // Joints
tMaterialArray m_materials; // Materials
btScalar m_timeacc; // Time accumulator
btVector3 m_bounds[2]; // Spatial bounds
bool m_bUpdateRtCst; // Update runtime constants
btDbvt m_ndbvt; // Nodes tree
btDbvt m_fdbvt; // Faces tree
btDbvntNode* m_fdbvnt; // Faces tree with normals
btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters
btScalar m_dampingCoefficient; // Damping Coefficient
btAlignedObjectArray<TetraScratch> m_tetraScratches;
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
tAnchorArray m_anchors; // Anchors
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
tSContactArray m_scontacts; // Soft contacts
tJointArray m_joints; // Joints
tMaterialArray m_materials; // Materials
btScalar m_timeacc; // Time accumulator
btVector3 m_bounds[2]; // Spatial bounds
bool m_bUpdateRtCst; // Update runtime constants
btDbvt m_ndbvt; // Nodes tree
btDbvt m_fdbvt; // Faces tree
btDbvntNode* m_fdbvnt; // Faces tree with normals
btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters
btScalar m_dampingCoefficient; // Damping Coefficient
btScalar m_sleepingThreshold;
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_gravityFactor;
btAlignedObjectArray<btVector3> m_X; // initial positions
btAlignedObjectArray<btVector3> m_X; // initial positions
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
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_softSoftCollision;
@ -858,11 +858,11 @@ public:
{
return m_worldInfo;
}
void setDampingCoefficient(btScalar damping_coeff)
{
m_dampingCoefficient = damping_coeff;
}
void setDampingCoefficient(btScalar damping_coeff)
{
m_dampingCoefficient = damping_coeff;
}
///@todo: avoid internal softbody shape hack and move collision code to collision library
virtual void setCollisionShape(btCollisionShape* collisionShape)
@ -923,9 +923,9 @@ public:
Material* mat = 0);
/* Append anchor */
void appendDeformableAnchor(int node, btRigidBody* body);
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
void appendAnchor(int node,
void appendDeformableAnchor(int node, btRigidBody* body);
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
void appendAnchor(int node,
btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void removeAnchor(int node);
@ -979,10 +979,10 @@ public:
void setLinearVelocity(const btVector3& linVel);
/* Set the angular velocity of the center of mass */
void setAngularVelocity(const btVector3& angVel);
/* Get best fit rigid transform */
btTransform getRigidTransform();
/* Transform to given pose */
void transformTo(const btTransform& trs);
/* Get best fit rigid transform */
btTransform getRigidTransform();
/* Transform to given pose */
void transformTo(const btTransform& trs);
/* Transform */
void transform(const btTransform& trs);
/* Translate */
@ -1071,11 +1071,11 @@ public:
/* defaultCollisionHandlers */
void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
void defaultCollisionHandler(btSoftBody* psb);
void setSelfCollision(bool useSelfCollision);
bool useSelfCollision();
void updateDeactivation(btScalar timeStep);
void setZeroVelocity();
bool wantsSleeping();
void setSelfCollision(bool useSelfCollision);
bool useSelfCollision();
void updateDeactivation(btScalar timeStep);
void setZeroVelocity();
bool wantsSleeping();
//
// Functionality to deal with new accelerated solvers.
@ -1154,8 +1154,8 @@ public:
void rebuildNodeTree();
btVector3 evaluateCom() 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 checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) 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;
void updateNormals();
void updateBounds();
void updatePose();
@ -1169,15 +1169,15 @@ public:
void solveClusters(btScalar sor);
void applyClusters(bool drift);
void dampClusters();
void setSpringStiffness(btScalar k);
void setSpringStiffness(btScalar k);
void setGravityFactor(btScalar gravFactor);
void initializeDmInverse();
void updateDeformation();
void advanceDeformation();
void initializeDmInverse();
void updateDeformation();
void advanceDeformation();
void applyForces();
void setMaxStress(btScalar maxStress);
void interpolateRenderMesh();
void setCollisionQuadrature(int N);
void setMaxStress(btScalar maxStress);
void interpolateRenderMesh();
void setCollisionQuadrature(int N);
static void PSolve_Anchors(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);
@ -1186,14 +1186,15 @@ public:
static psolver_t getSolver(ePSolver::_ solver);
static vsolver_t getSolver(eVSolver::_ solver);
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)
{
if (node->isleaf())
{
btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
if (use_velocity)
{
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[1], use_velocity, margin);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
node->volume = vol;
}
}
void updateNodeTree(bool use_velocity, bool margin)
void updateNodeTree(bool use_velocity, bool margin)
{
if (m_ndbvt.m_root)
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)
{
if (node->isleaf())
{
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
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
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,
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[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};
vol = btDbvtVolume::FromPoints(points, 6);
}
else
{
btVector3 points[3] = {f->m_n[0]->m_x,
f->m_n[1]->m_x,
f->m_n[2]->m_x};
f->m_n[1]->m_x,
f->m_n[2]->m_x};
vol = btDbvtVolume::FromPoints(points, 3);
}
vol.Expand(btVector3(pad, pad, pad));
@ -1253,7 +1256,8 @@ public:
{
updateFace(node->childs[0], 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);
node->volume = vol;
}
@ -1275,7 +1279,7 @@ public:
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;
{
@ -1301,60 +1305,60 @@ public:
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);
btScalar d = c.m_margin - n.dot(l);
d = btMax(btScalar(0),d);
d = btMax(btScalar(0), d);
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 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)
continue;
btVector3 vt = vr - vn*n;
btVector3 vt = vr - vn * n;
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)
I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
if (vn < 0)
I += 0.5 * mass * vn;
int face_penetration = 0, node_penetration = node->m_constrained;
for (int i = 0; i < 3; ++i)
face_penetration |= face->m_n[i]->m_constrained;
btScalar I_tilde = .5 *I /(1.0+w.length2());
// double the impulse if node or face is constrained.
if (face_penetration > 0 || node_penetration > 0)
face_penetration |= face->m_n[i]->m_constrained;
btScalar I_tilde = .5 * I / (1.0 + w.length2());
// double the impulse if node or face is constrained.
if (face_penetration > 0 || node_penetration > 0)
{
I_tilde *= 2.0;
}
if (face_penetration <= 0)
if (face_penetration <= 0)
{
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
btScalar vt_norm = vt.safeNorm();
if (vt_norm > SIMD_EPSILON)
{
btScalar delta_vn = -2 * I * node->m_im;
btScalar mu = c.m_friction;
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);
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);
vt.safeNormalize();
I_tilde = .5 *I /(1.0+w.length2());
// double the impulse if node or face is constrained.
// if (face_penetration > 0 || node_penetration > 0)
// I_tilde *= 2.0;
if (face_penetration <= 0)
I_tilde = .5 * I / (1.0 + w.length2());
// double the impulse if node or face is constrained.
// if (face_penetration > 0 || node_penetration > 0)
// I_tilde *= 2.0;
if (face_penetration <= 0)
{
for (int j = 0; j < 3; ++j)
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;
}
@ -1362,7 +1366,7 @@ public:
}
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
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 fixeds,
bool gendiags,
btScalar perturbation)
btScalar perturbation)
{
#define IDX(_x_, _y_) ((_y_)*rx + (_x_))
/* Create nodes */
@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
for (int ix = 0; ix < rx; ++ix)
{
const btScalar tx = ix / (btScalar)(rx - 1);
btScalar pert = perturbation * btScalar(rand())/RAND_MAX;
btVector3 temp1 = py1;
temp1.setY(py1.getY() + pert);
btVector3 temp = py0;
pert = perturbation * btScalar(rand())/RAND_MAX;
temp.setY(py0.getY() + pert);
btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
btVector3 temp1 = py1;
temp1.setY(py1.getY() + pert);
btVector3 temp = py0;
pert = perturbation * btScalar(rand()) / RAND_MAX;
temp.setY(py0.getY() + pert);
x[IDX(ix, iy)] = lerp(temp, temp1, tx);
m[IDX(ix, iy)] = 1;
}
@ -1233,9 +1233,9 @@ if(face&&face[0])
}
}
}
psb->initializeDmInverse();
psb->m_tetraScratches.resize(psb->m_tetras.size());
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
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());
@ -1245,61 +1245,61 @@ if(face&&face[0])
btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
{
std::ifstream fs;
fs.open(vtk_file);
btAssert(fs);
typedef btAlignedObjectArray<int> Index;
std::string line;
btAlignedObjectArray<btVector3> X;
btVector3 position;
btAlignedObjectArray<Index> indices;
bool reading_points = false;
bool reading_tets = false;
size_t n_points = 0;
size_t n_tets = 0;
size_t x_count = 0;
size_t indices_count = 0;
while (std::getline(fs, line))
{
std::stringstream ss(line);
if (line.size() == (size_t)(0))
{
}
else if (line.substr(0, 6) == "POINTS")
{
reading_points = true;
reading_tets = false;
ss.ignore(128, ' '); // ignore "POINTS"
ss >> n_points;
X.resize(n_points);
}
else if (line.substr(0, 5) == "CELLS")
{
reading_points = false;
reading_tets = true;
ss.ignore(128, ' '); // ignore "CELLS"
ss >> n_tets;
indices.resize(n_tets);
}
else if (line.substr(0, 10) == "CELL_TYPES")
{
reading_points = false;
reading_tets = false;
}
else if (reading_points)
{
btScalar p;
ss >> p;
position.setX(p);
ss >> p;
position.setY(p);
ss >> p;
position.setZ(p);
X[x_count++] = position;
}
else if (reading_tets)
{
std::ifstream fs;
fs.open(vtk_file);
btAssert(fs);
typedef btAlignedObjectArray<int> Index;
std::string line;
btAlignedObjectArray<btVector3> X;
btVector3 position;
btAlignedObjectArray<Index> indices;
bool reading_points = false;
bool reading_tets = false;
size_t n_points = 0;
size_t n_tets = 0;
size_t x_count = 0;
size_t indices_count = 0;
while (std::getline(fs, line))
{
std::stringstream ss(line);
if (line.size() == (size_t)(0))
{
}
else if (line.substr(0, 6) == "POINTS")
{
reading_points = true;
reading_tets = false;
ss.ignore(128, ' '); // ignore "POINTS"
ss >> n_points;
X.resize(n_points);
}
else if (line.substr(0, 5) == "CELLS")
{
reading_points = false;
reading_tets = true;
ss.ignore(128, ' '); // ignore "CELLS"
ss >> n_tets;
indices.resize(n_tets);
}
else if (line.substr(0, 10) == "CELL_TYPES")
{
reading_points = false;
reading_tets = false;
}
else if (reading_points)
{
btScalar p;
ss >> p;
position.setX(p);
ss >> p;
position.setY(p);
ss >> p;
position.setZ(p);
X[x_count++] = position;
}
else if (reading_tets)
{
int d;
ss >> d;
if (d != 4)
@ -1308,116 +1308,115 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
fs.close();
return 0;
}
ss.ignore(128, ' '); // ignore "4"
Index tet;
tet.resize(4);
for (size_t i = 0; i < 4; i++)
{
ss >> tet[i];
printf("%d ", tet[i]);
}
printf("\n");
indices[indices_count++] = tet;
}
}
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());
ss.ignore(128, ' '); // ignore "4"
Index tet;
tet.resize(4);
for (size_t i = 0; i < 4; i++)
{
ss >> tet[i];
printf("%d ", tet[i]);
}
printf("\n");
indices[indices_count++] = tet;
}
}
btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
fs.close();
return psb;
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();
return psb;
}
void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
{
int counter = 0;
for (int i = 0; i < psb->m_nodes.size(); ++i)
{
psb->m_nodes[i].index = counter++;
}
typedef btAlignedObjectArray<int> Index;
btAlignedObjectArray<Index> indices;
indices.resize(psb->m_tetras.size());
for (int i = 0; i < indices.size(); ++i)
{
Index 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[2]->index);
index.push_back(psb->m_tetras[i].m_n[3]->index);
indices[i] = index;
}
std::map<std::vector<int>, std::vector<int> > dict;
for (int i = 0; i < indices.size(); ++i)
{
for (int j = 0; j < 4; ++j)
{
std::vector<int> f;
if (j == 0)
{
f.push_back(indices[i][1]);
f.push_back(indices[i][0]);
f.push_back(indices[i][2]);
}
if (j == 1)
{
f.push_back(indices[i][3]);
f.push_back(indices[i][0]);
f.push_back(indices[i][1]);
}
if (j == 2)
{
f.push_back(indices[i][3]);
f.push_back(indices[i][1]);
f.push_back(indices[i][2]);
}
if (j == 3)
{
f.push_back(indices[i][2]);
f.push_back(indices[i][0]);
f.push_back(indices[i][3]);
}
std::vector<int> f_sorted = f;
std::sort(f_sorted.begin(), f_sorted.end());
if (dict.find(f_sorted) != dict.end())
{
dict.erase(f_sorted);
}
else
{
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)
{
std::vector<int> f = it->second;
psb->appendFace(f[0], f[1], f[2]);
}
int counter = 0;
for (int i = 0; i < psb->m_nodes.size(); ++i)
{
psb->m_nodes[i].index = counter++;
}
typedef btAlignedObjectArray<int> Index;
btAlignedObjectArray<Index> indices;
indices.resize(psb->m_tetras.size());
for (int i = 0; i < indices.size(); ++i)
{
Index 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[2]->index);
index.push_back(psb->m_tetras[i].m_n[3]->index);
indices[i] = index;
}
std::map<std::vector<int>, std::vector<int> > dict;
for (int i = 0; i < indices.size(); ++i)
{
for (int j = 0; j < 4; ++j)
{
std::vector<int> f;
if (j == 0)
{
f.push_back(indices[i][1]);
f.push_back(indices[i][0]);
f.push_back(indices[i][2]);
}
if (j == 1)
{
f.push_back(indices[i][3]);
f.push_back(indices[i][0]);
f.push_back(indices[i][1]);
}
if (j == 2)
{
f.push_back(indices[i][3]);
f.push_back(indices[i][1]);
f.push_back(indices[i][2]);
}
if (j == 3)
{
f.push_back(indices[i][2]);
f.push_back(indices[i][0]);
f.push_back(indices[i][3]);
}
std::vector<int> f_sorted = f;
std::sort(f_sorted.begin(), f_sorted.end());
if (dict.find(f_sorted) != dict.end())
{
dict.erase(f_sorted);
}
else
{
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)
{
std::vector<int> f = it->second;
psb->appendFace(f[0], f[1], f[2]);
}
}
//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)
{
// 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;
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)
{
fs << "f";
@ -1462,7 +1461,7 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
}
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)
{
fs << "v";
@ -1488,176 +1487,175 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
{
std::ifstream fs_read;
fs_read.open(filename);
std::string line;
btVector3 pos;
btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
while (std::getline(fs_read, line))
{
std::stringstream ss(line);
if (line[0] == 'v')
{
}
else if (line[0] == 'f')
{
ss.ignore();
int id0, id1, id2;
ss >> id0;
ss >> id1;
ss >> id2;
btAlignedObjectArray<int> new_face;
new_face.push_back(id1);
new_face.push_back(id0);
new_face.push_back(id2);
additional_faces.push_back(new_face);
}
}
fs_read.close();
std::ifstream fs_read;
fs_read.open(filename);
std::string line;
btVector3 pos;
btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
while (std::getline(fs_read, line))
{
std::stringstream ss(line);
if (line[0] == 'v')
{
}
else if (line[0] == 'f')
{
ss.ignore();
int id0, id1, id2;
ss >> id0;
ss >> id1;
ss >> id2;
btAlignedObjectArray<int> new_face;
new_face.push_back(id1);
new_face.push_back(id0);
new_face.push_back(id2);
additional_faces.push_back(new_face);
}
}
fs_read.close();
std::ofstream fs_write;
fs_write.open(filename, std::ios_base::app);
for (int i = 0; i < additional_faces.size(); ++i)
{
fs_write << "f";
for (int n = 0; n < 3; n++)
{
fs_write << " " << additional_faces[i][n];
}
fs_write << "\n";
}
fs_write.close();
std::ofstream fs_write;
fs_write.open(filename, std::ios_base::app);
for (int i = 0; i < additional_faces.size(); ++i)
{
fs_write << "f";
for (int n = 0; n < 3; n++)
{
fs_write << " " << additional_faces[i][n];
}
fs_write << "\n";
}
fs_write.close();
}
// 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)
{
btVector3 vap = p - a;
btVector3 vbp = p - b;
btVector3 vab = b - a;
btVector3 vac = c - a;
btVector3 vad = d - a;
btVector3 vbc = c - b;
btVector3 vbd = d - b;
btScalar va6 = (vbp.cross(vbd)).dot(vbc);
btScalar vb6 = (vap.cross(vac)).dot(vad);
btScalar vc6 = (vap.cross(vad)).dot(vab);
btScalar vd6 = (vap.cross(vab)).dot(vac);
btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
btVector3 vap = p - a;
btVector3 vbp = p - b;
btVector3 vab = b - a;
btVector3 vac = c - a;
btVector3 vad = d - a;
btVector3 vbc = c - b;
btVector3 vbd = d - b;
btScalar va6 = (vbp.cross(vbd)).dot(vbc);
btScalar vb6 = (vap.cross(vac)).dot(vad);
btScalar vc6 = (vap.cross(vad)).dot(vab);
btScalar vd6 = (vap.cross(vab)).dot(vac);
btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
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.
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;
btScalar d00 = btDot(v0, v0);
btScalar d01 = btDot(v0, v1);
btScalar d11 = btDot(v1, v1);
btScalar d20 = btDot(v2, v0);
btScalar d21 = btDot(v2, v1);
btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
bary[1] = (d11 * d20 - d01 * d21) * invDenom;
bary[2] = (d00 * d21 - d01 * d20) * invDenom;
bary[0] = 1.0 - bary[1] - bary[2];
bary[3] = 0;
btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
btScalar d00 = btDot(v0, v0);
btScalar d01 = btDot(v0, v1);
btScalar d11 = btDot(v1, v1);
btScalar d20 = btDot(v2, v0);
btScalar d21 = btDot(v2, v1);
btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
bary[1] = (d11 * d20 - d01 * d21) * invDenom;
bary[2] = (d00 * d21 - d01 * d20) * invDenom;
bary[0] = 1.0 - bary[1] - bary[2];
bary[3] = 0;
}
// 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
void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
{
psb->m_z.resize(0);
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
{
const btVector3& p = psb->m_renderNodes[i].m_x;
btVector4 bary;
btVector4 optimal_bary;
btScalar min_bary_weight = -1e3;
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
for (int j = 0; j < psb->m_tetras.size(); ++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);
btScalar new_min_bary_weight = bary[0];
for (int k = 1; k < 4; ++k)
{
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
}
if (new_min_bary_weight > min_bary_weight)
{
btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(t.m_n[0]);
parents.push_back(t.m_n[1]);
parents.push_back(t.m_n[2]);
parents.push_back(t.m_n[3]);
optimal_parents = parents;
optimal_bary = bary;
min_bary_weight = new_min_bary_weight;
// stop searching if p is inside the tetrahedron at hand
if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.)
{
break;
}
}
}
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
psb->m_renderNodesParents[i] = optimal_parents;
}
psb->m_z.resize(0);
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
{
const btVector3& p = psb->m_renderNodes[i].m_x;
btVector4 bary;
btVector4 optimal_bary;
btScalar min_bary_weight = -1e3;
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
for (int j = 0; j < psb->m_tetras.size(); ++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);
btScalar new_min_bary_weight = bary[0];
for (int k = 1; k < 4; ++k)
{
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
}
if (new_min_bary_weight > min_bary_weight)
{
btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(t.m_n[0]);
parents.push_back(t.m_n[1]);
parents.push_back(t.m_n[2]);
parents.push_back(t.m_n[3]);
optimal_parents = parents;
optimal_bary = bary;
min_bary_weight = new_min_bary_weight;
// stop searching if p is inside the tetrahedron at hand
if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
{
break;
}
}
}
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
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.
void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
{
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
psb->m_z.resize(psb->m_renderNodes.size());
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
{
const btVector3& p = psb->m_renderNodes[i].m_x;
btVector4 bary;
btVector4 optimal_bary;
btScalar min_bary_weight = -SIMD_INFINITY;
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
btScalar dist = 0, optimal_dist = 0;
for (int j = 0; j < psb->m_faces.size(); ++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 unit_n = n.normalized();
dist = (p-f.m_n[0]->m_x).dot(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);
btScalar new_min_bary_weight = bary[0];
for (int k = 1; k < 3; ++k)
{
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
}
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
psb->m_z.resize(psb->m_renderNodes.size());
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
{
const btVector3& p = psb->m_renderNodes[i].m_x;
btVector4 bary;
btVector4 optimal_bary;
btScalar min_bary_weight = -SIMD_INFINITY;
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
btScalar dist = 0, optimal_dist = 0;
for (int j = 0; j < psb->m_faces.size(); ++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 unit_n = n.normalized();
dist = (p - f.m_n[0]->m_x).dot(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);
btScalar new_min_bary_weight = bary[0];
for (int k = 1; k < 3; ++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
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
bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist));
// 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.);
// 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));
if (better_than_closest_outisde || better_than_best_inside)
{
btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(f.m_n[0]);
parents.push_back(f.m_n[1]);
parents.push_back(f.m_n[2]);
optimal_parents = parents;
optimal_bary = bary;
optimal_dist = dist;
min_bary_weight = new_min_bary_weight;
}
}
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
psb->m_renderNodesParents[i] = optimal_parents;
psb->m_z[i] = optimal_dist;
}
if (better_than_closest_outisde || better_than_best_inside)
{
btAlignedObjectArray<const btSoftBody::Node*> parents;
parents.push_back(f.m_n[0]);
parents.push_back(f.m_n[1]);
parents.push_back(f.m_n[2]);
optimal_parents = parents;
optimal_bary = bary;
optimal_dist = dist;
min_bary_weight = new_min_bary_weight;
}
}
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
psb->m_renderNodesParents[i] = optimal_parents;
psb->m_z[i] = optimal_dist;
}
}

30
src/BulletSoftBody/btSoftBodyHelpers.h
View File

@ -93,7 +93,7 @@ struct btSoftBodyHelpers
int resy,
int fixeds,
bool gendiags,
btScalar perturbation = 0.);
btScalar perturbation = 0.);
/* Create a patch with UV Texture Coordinates */
static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00,
@ -142,21 +142,21 @@ struct btSoftBodyHelpers
bool bfacelinks,
bool btetralinks,
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 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 interpolateBarycentricWeights(btSoftBody* psb);
static void extrapolateBarycentricWeights(btSoftBody* psb);
static void generateBoundaryFaces(btSoftBody* psb);
static void duplicateFaces(const char* filename, 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& p, btVector4& bary);
static void interpolateBarycentricWeights(btSoftBody* psb);
static void extrapolateBarycentricWeights(btSoftBody* psb);
static void generateBoundaryFaces(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
/// ones as far as possible away from the calculation of those values
/// 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,
DX_SOLVER,
DX_SIMD_SOLVER,
DEFORMABLE_SOLVER
DEFORMABLE_SOLVER
};
protected:

68
src/BulletSoftBody/btSparseSDF.h
View File

@ -22,36 +22,36 @@ subject to the following restrictions:
// Fast Hash
#if !defined (get16bits)
#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\
+(unsigned int)(((const unsigned char *)(d))[0]) )
#if !defined(get16bits)
#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
#endif
//
// super hash function by Paul Hsieh
//
inline unsigned int HsiehHash (const char * data, int len) {
unsigned int hash = len, tmp;
len>>=2;
inline unsigned int HsiehHash(const char* data, int len)
{
unsigned int hash = len, tmp;
len >>= 2;
/* Main loop */
for (;len > 0; len--) {
hash += get16bits (data);
tmp = (get16bits (data+2) << 11) ^ hash;
hash = (hash << 16) ^ tmp;
data += 2*sizeof (unsigned short);
hash += hash >> 11;
}
/* Main loop */
for (; len > 0; len--)
{
hash += get16bits(data);
tmp = (get16bits(data + 2) << 11) ^ hash;
hash = (hash << 16) ^ tmp;
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 */
hash ^= hash << 3;
hash += hash >> 5;
hash ^= hash << 4;
hash += hash >> 17;
hash ^= hash << 25;
hash += hash >> 6;
return hash;
return hash;
}
template <const int CELLSIZE>
@ -81,7 +81,7 @@ struct btSparseSdf
btAlignedObjectArray<Cell*> cells;
btScalar voxelsz;
btScalar m_defaultVoxelsz;
btScalar m_defaultVoxelsz;
int puid;
int ncells;
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)
m_clampCells = clampCells;
cells.resize(hashsize, 0);
m_defaultVoxelsz = 0.25;
m_defaultVoxelsz = 0.25;
Reset();
}
//
void setDefaultVoxelsz(btScalar sz)
{
m_defaultVoxelsz = sz;
}
void setDefaultVoxelsz(btScalar sz)
{
m_defaultVoxelsz = sz;
}
void Reset()
{
for (int i = 0, ni = cells.size(); i < ni; ++i)
@ -162,7 +162,7 @@ struct btSparseSdf
nqueries = 1;
nprobes = 1;
++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)
@ -221,7 +221,7 @@ struct btSparseSdf
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("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;
}
@ -363,7 +363,7 @@ struct btSparseSdf
myset.p = (void*)shape;
const char* ptr = (const char*)&myset;
unsigned int result = HsiehHash(ptr, sizeof(btS) );
unsigned int result = HsiehHash(ptr, sizeof(btS));
return result;
}

742
src/BulletSoftBody/poly34.cpp
View File

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

18
src/BulletSoftBody/poly34.h
View File

@ -8,31 +8,31 @@
// x - array of size 2
// return 2: 2 real roots x[0], 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
// 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]
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
// 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 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
// 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 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.
// 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 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)
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
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
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 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