AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +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

3
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);
}
return m_isConnected;
}

102
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -93,7 +93,6 @@
#include "../TinyAudio/b3SoundEngine.h"
#endif
#ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#define SKIP_DEFORMABLE_BODY 1
#endif
@ -117,8 +116,6 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gVRTrackingObjectUniqueId = -1;
@ -1617,13 +1614,10 @@ 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;
b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
@ -1653,7 +1647,6 @@ struct PhysicsServerCommandProcessorInternalData
btDefaultCollisionConfiguration* m_collisionConfiguration;
#ifndef SKIP_DEFORMABLE_BODY
btSoftBody* m_pickedSoftBody;
btDeformableMousePickingForce* m_mouseForce;
@ -2672,7 +2665,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
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);
@ -2899,7 +2893,6 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
}
}
#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,11 +3062,14 @@ 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);
}
@ -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);
@ -5304,7 +5306,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
{
//collision shape
if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1)
{
colShape = bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape();
@ -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,9 +5974,6 @@ struct CastSyncInfo
};
#endif // __cplusplus >= 201103L
struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask)
@ -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;
}
@ -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]);
}
}
@ -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;
}
@ -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] =
@ -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)
@ -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,10 +9834,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
gforce->m_gravity = grav;
}
}
}
#endif
if (m_data->m_verboseOutput)
{
@ -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;
@ -11040,6 +11031,8 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
}
}
}
#endif
}
SharedMemoryStatus& serverCmd = serverStatusOut;
@ -11283,12 +11276,12 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
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)
{
@ -11296,7 +11289,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
if (linkIndex < 0)
{
sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider());
} else
}
else
{
if (linkIndex < mbodyHandle->m_multiBody->getNumLinks())
{
@ -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;
}
@ -13787,8 +13778,6 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
}
}
struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
int m_faceId;
@ -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)

1
src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h
View File

@ -18,7 +18,6 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
{
}
virtual void processConstraints(int islandId = -1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;

4
src/BulletSoftBody/btCGProjection.h
View File

@ -75,8 +75,7 @@ public:
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt)
: m_softBodies(softBodies)
, m_dt(dt)
: m_softBodies(softBodies), m_dt(dt)
{
}
@ -102,5 +101,4 @@ public:
}
};
#endif /* btCGProjection_h */

10
src/BulletSoftBody/btConjugateGradient.h
View File

@ -22,6 +22,7 @@ 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)
@ -44,7 +45,8 @@ public:
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = this->dot(z, r);
if (r_dot_z <= Base::m_tolerance) {
if (r_dot_z <= Base::m_tolerance)
{
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
@ -54,7 +56,8 @@ public:
}
p = z;
btScalar r_dot_z_new = r_dot_z;
for (int k = 1; k <= Base::m_maxIterations; k++) {
for (int k = 1; k <= Base::m_maxIterations; k++)
{
// temp = A*p
A.multiply(p, temp);
A.project(temp);
@ -78,7 +81,8 @@ public:
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 (r_dot_z_new < Base::m_tolerance)
{
if (verbose)
{
std::cout << "ConjugateGradient iterations " << k << std::endl;

11
src/BulletSoftBody/btConjugateResidual.h
View File

@ -27,6 +27,7 @@ class btConjugateResidual : public btKrylovSolver<MatrixX>
// 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)
@ -48,7 +49,8 @@ public:
A.precondition(r, z); // borrow z to store preconditioned r
r = z;
btScalar residual_norm = this->norm(r);
if (residual_norm <= Base::m_tolerance) {
if (residual_norm <= Base::m_tolerance)
{
return 0;
}
p = r;
@ -58,7 +60,8 @@ public:
// 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++) {
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)
@ -72,7 +75,8 @@ public:
{
best_x = x;
best_r = norm_r;
if (norm_r < Base::m_tolerance) {
if (norm_r < Base::m_tolerance)
{
return k;
}
}
@ -106,4 +110,3 @@ public:
}
};
#endif /* btConjugateResidual_h */

5
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -18,10 +18,7 @@
#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)
: 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);

17
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -20,13 +20,7 @@
#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);
}
@ -95,9 +89,11 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
btScalar scale = 2;
btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
backupDv();
do {
do
{
scale *= beta;
if (scale < 1e-8) {
if (scale < 1e-8)
{
return;
}
updateEnergy(scale);
@ -166,7 +162,6 @@ void btDeformableBodySolver::updateEnergy(btScalar scale)
updateState();
}
btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
{
m_cg.solve(*m_objective, ddv, residual, false);
@ -378,7 +373,6 @@ bool btDeformableBodySolver::updateNodes()
return false;
}
void btDeformableBodySolver::predictMotion(btScalar solverdt)
{
// apply explicit forces to velocity
@ -455,7 +449,6 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
// psb->m_fdbvt.optimizeIncremental(1);
}
void btDeformableBodySolver::updateSoftBodies()
{
BT_PROFILE("updateSoftBodies");

5
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"
@ -31,6 +30,7 @@ class btDeformableMultiBodyDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
{
typedef btAlignedObjectArray<btVector3> TVStack;
protected:
int m_numNodes; // total number of deformable body nodes
TVStack m_dv; // v_{n+1} - v_n
@ -111,7 +111,8 @@ public:
}
// process collision between deformable and deformable
virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody)
{
softBody->defaultCollisionHandler(otherSoftBody);
}

38
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -16,14 +16,12 @@
#include "btDeformableContactConstraint.h"
/* ================ Deformable Node Anchor =================== */
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
: m_anchor(&a)
, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
: m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
{
}
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
: m_anchor(other.m_anchor)
, btDeformableContactConstraint(other)
: m_anchor(other.m_anchor), btDeformableContactConstraint(other)
{
}
@ -135,8 +133,7 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
/* ================ Deformable vs. Rigid =================== */
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
: m_contact(&c)
, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
: m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
@ -147,17 +144,12 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
, m_penetration(other.m_penetration)
, m_total_split_impulse(other.m_total_split_impulse)
, m_binding(other.m_binding)
: m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
}
btVector3 btDeformableRigidContactConstraint::getVa() const
{
const btSoftBody::sCti& cti = m_contact->m_cti;
@ -416,14 +408,12 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)
, btDeformableRigidContactConstraint(contact, infoGlobal)
: m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal)
{
}
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
: m_node(other.m_node)
, btDeformableRigidContactConstraint(other)
: m_node(other.m_node), btDeformableRigidContactConstraint(other)
{
}
@ -458,16 +448,12 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3&
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
: m_face(contact.m_face)
, m_useStrainLimiting(useStrainLimiting)
, btDeformableRigidContactConstraint(contact, infoGlobal)
: m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal)
{
}
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
: m_face(other.m_face)
, m_useStrainLimiting(other.m_useStrainLimiting)
, btDeformableRigidContactConstraint(other)
: m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other)
{
}
@ -478,7 +464,6 @@ btVector3 btDeformableFaceRigidContactConstraint::getVb() const
return vb;
}
btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
@ -571,7 +556,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
}
}
btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
@ -604,13 +588,9 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3&
}
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)
, m_face(contact.m_face)
, m_contact(&contact)
, btDeformableContactConstraint(contact.m_normal, infoGlobal)
: m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal)
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();

8
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -40,9 +40,7 @@ public:
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)
{
}
@ -80,8 +78,7 @@ public:
}
btDeformableStaticConstraint() {}
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
: m_node(other.m_node)
, btDeformableContactConstraint(other)
: m_node(other.m_node), btDeformableContactConstraint(other)
{
}
@ -139,7 +136,6 @@ public:
virtual void setPenetrationScale(btScalar scale) {}
};
//
// Constraint between rigid/multi body and deformable objects
class btDeformableRigidContactConstraint : public btDeformableContactConstraint

5
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -409,7 +409,6 @@ void btDeformableContactProjection::setProjection()
{
for (int l = 0; l < 3; ++l)
{
btReducedVector rv(dof);
for (int k = 0; k < 3; ++k)
{
@ -617,7 +616,6 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
m_nodeRigidConstraints.resize(N);
m_faceRigidConstraints.resize(N);
m_deformableConstraints.resize(N);
}
for (int i = 0; i < N; ++i)
{
@ -634,6 +632,3 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
#endif
m_lagrangeMultipliers.clear();
}

1
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -34,7 +34,6 @@ struct LagrangeMultiplier
int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes;
};
class btDeformableContactProjection
{
public:

3
src/BulletSoftBody/btDeformableCorotatedForce.h
View File

@ -32,7 +32,6 @@ public:
btScalar m_mu, m_lambda;
btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
{
}
btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
@ -120,8 +119,6 @@ public:
{
return BT_COROTATED_FORCE;
}
};
#endif /* btCorotated_h */

2
src/BulletSoftBody/btDeformableGravityForce.h
View File

@ -101,7 +101,5 @@ public:
}
return e;
}
};
#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

3
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@ -178,7 +178,6 @@ public:
dphi += dphi_dx[i].dot(dx[i]);
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
@ -241,7 +240,6 @@ public:
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack df;
@ -251,7 +249,6 @@ public:
TVStack f2;
f2.resize(dx.size());
// write down the current position
TVStack x;
x.resize(dx.size());

1
src/BulletSoftBody/btDeformableLinearElasticityForce.h
View File

@ -335,6 +335,5 @@ public:
{
return BT_LINEAR_ELASTICITY_FORCE;
}
};
#endif /* BT_LINEAR_ELASTICITY_H */

2
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@ -24,6 +24,7 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce
// 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)
@ -295,7 +296,6 @@ public:
{
return BT_MASSSPRING_FORCE;
}
};
#endif /* btMassSpring_h */

2
src/BulletSoftBody/btDeformableMousePickingForce.h
View File

@ -26,6 +26,7 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce
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)
@ -139,7 +140,6 @@ public:
{
return BT_MOUSE_PICKING_FORCE;
}
};
#endif /* btMassSpring_h */

1
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -13,7 +13,6 @@
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

2
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
@ -47,6 +46,7 @@ protected:
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();

10
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -41,7 +41,8 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
#include "btSoftBodyInternals.h"
btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
m_deformableBodySolver(deformableBodySolver),
m_solverCallback(0)
{
m_drawFlags = fDrawFlags::Std;
m_drawNodeTree = true;
@ -439,7 +440,6 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints()
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
}
void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
{
// process constraints on each island
@ -541,13 +541,10 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
}
}
void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
{
btMultiBodyDynamicsWorld::debugDrawWorld();
for (int i = 0; i < getSoftBodyArray().size(); i++)
@ -558,8 +555,6 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
}
}
}
void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
@ -740,7 +735,6 @@ void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject
btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
}
int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
{
startProfiling(timeStep);

2
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@ -240,8 +240,6 @@ public:
}
};
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
{
BT_PROFILE("rayTest");

1
src/BulletSoftBody/btDeformableNeoHookeanForce.h
View File

@ -416,6 +416,5 @@ public:
{
return BT_NEOHOOKEAN_FORCE;
}
};
#endif /* BT_NEOHOOKEAN_H */

5
src/BulletSoftBody/btKrylovSolver.h
View File

@ -27,14 +27,13 @@ template <class MatrixX>
class btKrylovSolver
{
typedef btAlignedObjectArray<btVector3> TVStack;
public:
int m_maxIterations;
btScalar m_tolerance;
btKrylovSolver(int maxIterations, btScalar tolerance)
: m_maxIterations(maxIterations)
, m_tolerance(tolerance)
: m_maxIterations(maxIterations), m_tolerance(tolerance)
{
}
virtual ~btKrylovSolver() {}

9
src/BulletSoftBody/btPreconditioner.h
View File

@ -45,6 +45,7 @@ class MassPreconditioner : public Preconditioner
{
btAlignedObjectArray<btScalar> m_inv_mass;
const btAlignedObjectArray<btSoftBody*>& m_softBodies;
public:
MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
: m_softBodies(softBodies)
@ -80,7 +81,6 @@ public:
}
};
class KKTPreconditioner : public Preconditioner
{
const btAlignedObjectArray<btSoftBody*>& m_softBodies;
@ -89,13 +89,10 @@ class KKTPreconditioner : public Preconditioner
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)
: m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
{
}

17
src/BulletSoftBody/btSoftBody.cpp
View File

@ -576,8 +576,6 @@ void btSoftBody::removeAnchor(int node)
}
}
//
void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
{
@ -2186,7 +2184,6 @@ void btSoftBody::predictMotion(btScalar dt)
m_cdbvt.optimizeIncremental(1);
}
//
void btSoftBody::solveConstraints()
{
@ -2575,7 +2572,6 @@ int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
return (cnt);
}
//
static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
{
@ -2633,7 +2629,8 @@ void btSoftBody::initializeFaceTree()
for (int i = 0; i < m_faces.size(); ++i)
{
Face& f = m_faces[i];
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol = VolumeOf(f, 0);
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
node->parent = NULL;
node->data = &f;
@ -2685,7 +2682,8 @@ void btSoftBody::rebuildNodeTree()
for (int i = 0; i < m_nodes.size(); ++i)
{
Node& n = m_nodes[i];
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol = btDbvtVolume::FromCR(n.m_x, 0);
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
node->parent = NULL;
node->data = &n;
@ -2766,8 +2764,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
@ -2816,8 +2813,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
btScalar dst;
@ -4652,7 +4648,6 @@ void btSoftBody::updateDeactivation(btScalar timeStep)
}
}
void btSoftBody::setZeroVelocity()
{
for (int i = 0; i < m_nodes.size(); ++i)

12
src/BulletSoftBody/btSoftBody.h
View File

@ -1192,7 +1192,8 @@ public:
if (node->isleaf())
{
btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
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)
{
@ -1211,7 +1212,8 @@ 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;
}
@ -1230,7 +1232,8 @@ public:
{
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;
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,
@ -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;
}

2
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@ -1336,7 +1336,6 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
}
}
generateBoundaryFaces(psb);
psb->initializeDmInverse();
psb->m_tetraScratches.resize(psb->m_tetras.size());
@ -1610,7 +1609,6 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
}
}
// 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)
{

27
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -103,8 +103,7 @@ static btVector3 dop[KDOP_COUNT]={btVector3(1,0,0),
btVector3(1, 1, 1),
btVector3(1, -1, 1),
btVector3(1, 1, -1),
btVector3(1,-1,-1)
};
btVector3(1, -1, -1)};
static inline int getSign(const btVector3& n, const btVector3& x)
{
@ -123,8 +122,7 @@ static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, c
face->m_n[2]->m_x - node->m_x,
face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x,
face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x,
face->m_n[2]->m_x + dt*face->m_n[2]->m_v - node->m_x
};
face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x};
btVector3 segment = dt * node->m_v;
for (int i = 0; i < KDOP_COUNT; ++i)
{
@ -171,34 +169,40 @@ inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btS
}
static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1)
{
if (sameSign(t0, t1)) {
if (sameSign(t0, t1))
{
lt0 = t0;
lt1 = t0;
return true;
}
if (type_c || diffSign(k0, k3)) {
if (type_c || diffSign(k0, k3))
{
btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
if (t0 < -0)
ft = -ft;
if (sameSign(ft, k0)) {
if (sameSign(ft, k0))
{
lt0 = t1;
lt1 = t1;
}
else {
else
{
lt0 = t0;
lt1 = t0;
}
return true;
}
if (!type_c) {
if (!type_c)
{
btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
if (t0 < -0)
ft = -ft;
if (diffSign(ft, k0)) {
if (diffSign(ft, k0))
{
lt0 = t0;
lt1 = t1;
return true;
@ -958,7 +962,6 @@ static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2)
return (m);
}
//
static inline btMatrix3x3 Add(const btMatrix3x3& a,
const btMatrix3x3& b)
@ -1158,7 +1161,6 @@ static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVec
return ret;
}
//
template <typename T>
static inline T BaryEval(const T& a,
@ -1890,7 +1892,6 @@ struct btSoftColliders
btScalar mrg;
};
//
// CollideVF_DD
//

10
src/BulletSoftBody/btSparseSDF.h
View File

@ -23,18 +23,19 @@ 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]) )
#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) {
inline unsigned int HsiehHash(const char* data, int len)
{
unsigned int hash = len, tmp;
len >>= 2;
/* Main loop */
for (;len > 0; len--) {
for (; len > 0; len--)
{
hash += get16bits(data);
tmp = (get16bits(data + 2) << 11) ^ hash;
hash = (hash << 16) ^ tmp;
@ -42,7 +43,6 @@ inline unsigned int HsiehHash (const char * data, int len) {
hash += hash >> 11;
}
/* Force "avalanching" of final 127 bits */
hash ^= hash << 3;
hash += hash >> 5;

84
src/BulletSoftBody/poly34.cpp
View File

@ -16,11 +16,13 @@ const btScalar eps = SIMD_EPSILON;
static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
{
btScalar s = 1.;
while (x < 1.) {
while (x < 1.)
{
x *= 8.;
s *= 0.5;
}
while (x > 8.) {
while (x > 8.)
{
x *= 0.125;
s *= 2.;
}
@ -50,7 +52,8 @@ btScalar SIMD_FORCE_INLINE root3(btScalar x)
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) {
if (D >= 0)
{
D = sqrt(D);
x[0] = -0.5 * a + D;
x[1] = -0.5 * a - D;
@ -76,7 +79,8 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
btScalar r2 = r * r;
btScalar q3 = q * q * q;
btScalar A, B;
if (r2 <= (q3 + eps)) { //<<-- FIXED!
if (r2 <= (q3 + eps))
{ //<<-- FIXED!
btScalar t = r / sqrt(q3);
if (t < -1)
t = -1;
@ -90,7 +94,8 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
x[2] = q * cos((t - TwoPi) / 3) - a;
return (3);
}
else {
else
{
//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
A = -root3(fabs(r) + sqrt(r2 - q3));
if (r < 0)
@ -101,7 +106,8 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
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) {
if (fabs(x[2]) < eps)
{
x[2] = x[1];
return (2);
}
@ -113,18 +119,22 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
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) {
if (y == 0)
{
r = sqrt(r);
if (x >= 0) {
if (x >= 0)
{
a = r;
b = 0;
}
else {
else
{
a = 0;
b = r;
}
}
else { // y != 0
else
{ // y != 0
a = sqrt(0.5 * (x + r));
b = 0.5 * y / a;
}
@ -133,7 +143,8 @@ void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s
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) {
if (D >= 0)
{
btScalar sD = sqrt(D);
btScalar x1 = (-b + sD) / 2;
btScalar x2 = (-b - sD) / 2; // x2 <= x1
@ -166,7 +177,8 @@ int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2
x[3] = sx2;
return 2;
}
else { // if( D < 0 ), two pair of compex roots
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]);
@ -185,7 +197,8 @@ 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) {
if (c < b)
{
SWAP(b, c); // now a<=b, b<=c
if (a > b)
SWAP(a, b); // now a<=b
@ -210,7 +223,8 @@ int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation
btScalar sz2 = sqrt(x[1]);
btScalar sz3 = sqrt(x[2]);
// Note: sz1*sz2*sz3= -c (and not equal to 0)
if (c > 0) {
if (c > 0)
{
x[0] = (-sz1 - sz2 - sz3) / 2;
x[1] = (-sz1 + sz2 + sz3) / 2;
x[2] = (+sz1 - sz2 + sz3) / 2;
@ -290,27 +304,32 @@ int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
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) {
if (res == 4)
{
x[0] -= a / 4;
x[1] -= a / 4;
x[2] -= a / 4;
x[3] -= a / 4;
}
else if (res == 2) {
else if (res == 2)
{
x[0] -= a / 4;
x[1] -= a / 4;
x[2] -= a / 4;
}
else {
else
{
x[0] -= a / 4;
x[2] -= a / 4;
}
// one Newton step for each real root:
if (res > 0) {
if (res > 0)
{
x[0] = N4Step(x[0], a, b, c, d);
x[1] = N4Step(x[1], a, b, c, d);
}
if (res > 2) {
if (res > 2)
{
x[2] = N4Step(x[2], a, b, c, d);
x[3] = N4Step(x[3], a, b, c, d);
}
@ -341,14 +360,16 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
btScalar x2, f2, f2s; // next values, f(x2), f'(x2)
btScalar dx = 0;
if (e < 0) {
if (e < 0)
{
x0 = 0;
x1 = brd;
f0 = e;
f1 = F5(x1);
x2 = 0.01 * brd;
} // positive root
else {
else
{
x0 = -brd;
x1 = 0;
f0 = F5(x0);
@ -363,17 +384,20 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
// now x0<x1, f(x0)<0, f(x1)>0
// Firstly 10 bisections
for (cnt = 0; cnt < 10; cnt++) {
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) {
if (f2 > 0)
{
x1 = x2;
f1 = f2;
}
else {
else
{
x0 = x2;
f0 = f2;
}
@ -383,7 +407,8 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
// x0<x1, f(x0)<0, f(x1)>0.
// x2 - next value
// we hope that x0 < x2 < x1, but not necessarily
do {
do
{
if (cnt++ > 50)
break;
if (x2 <= x0 || x2 >= x1)
@ -391,16 +416,19 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
f2 = F5(x2); // f(x2)
if (fabs(f2) < eps)
return x2;
if (f2 > 0) {
if (f2 > 0)
{
x1 = x2;
f1 = f2;
}
else {
else
{
x0 = x2;
f0 = f2;
}
f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
if (fabs(f2s) < eps) {
if (fabs(f2s) < eps)
{
x2 = 1e99;
continue;
}