Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2019-11-04 15:46:23 -08:00
parent f1f04aef53 aac7370179
commit b5715c96cf
27 changed files with 3051 additions and 2832 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3025
Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
View File

File diff suppressed because it is too large Load Diff

12
README.md
View File

@@ -44,6 +44,18 @@ track down the issue, but more work is required to cover all OpenCL kernels.
All source code files are licensed under the permissive zlib license
(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
## Build instructions for Bullet using vcpkg
You can download and install Bullet using the [vcpkg](https://github.com/Microsoft/vcpkg/) dependency manager:
git clone https://github.com/Microsoft/vcpkg.git
cd vcpkg
./bootstrap-vcpkg.sh
./vcpkg integrate install
vcpkg install bullet3
The Bullet port in vcpkg is kept up to date by Microsoft team members and community contributors. If the version is out of date, please [create an issue or pull request](https://github.com/Microsoft/vcpkg) on the vcpkg repository.
## Build instructions for Bullet using premake. You can also use cmake instead.
**Windows**

2
VERSION
View File

@@ -1 +1 @@
2.88
2.89

8
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -752,6 +752,14 @@ B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandl
return 0;
}
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_articulatedWarmStartingFactor = warmStartingFactor;
command->m_updateFlags |= SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -339,6 +339,7 @@ extern "C"
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);

2
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1458,7 +1458,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName;
if (bf.ok())
{

2
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -1218,7 +1218,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName;
break;
}
case CMD_SYNC_USER_DATA_FAILED:

22
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3463,8 +3463,6 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
ser.startSerialization();
ser.registerNameForPointer(sb, bodyHandle->m_bodyName.c_str());
int len = sb->calculateSerializeBufferSize();
btChunk* chunk = ser.allocate(len, 1);
const char* structType = sb->serialize(chunk->m_oldPtr, &ser);
@@ -7259,6 +7257,7 @@ 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] =
@@ -7275,11 +7274,12 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3];
btVector3 center_of_mass(sb->getCenterOfMass());
btTransform tr = sb->getWorldTransform();
//base position in world space, cartesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
stateDetails->m_actualStateQ[0] = center_of_mass[0];
stateDetails->m_actualStateQ[1] = center_of_mass[1];
stateDetails->m_actualStateQ[2] = center_of_mass[2];
//base orientation, quaternion x,y,z,w, in world space, cartesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
@@ -8227,6 +8227,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
int pos = strlen(relativeFileName)-1;
while(pos>=0 && relativeFileName[pos]!='/') { pos--;}
btAssert(strlen(relativeFileName)-pos-5>0);
std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos));
bodyHandle->m_bodyName = object_name;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
@@ -9338,6 +9344,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = clientCmd.m_physSimParamArgs.m_warmStartingFactor;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR)
{
m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_USE_ARTICULATED_WARMSTARTING;
m_data->m_dynamicsWorld->getSolverInfo().m_articulatedWarmstartingFactor = clientCmd.m_physSimParamArgs.m_articulatedWarmStartingFactor;
}
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -483,6 +483,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25,
SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28,
};

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -947,6 +947,7 @@ struct b3PhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
double m_warmStartingFactor;
double m_articulatedWarmStartingFactor;
int m_useRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;

4
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_extended_env.py
View File

@@ -122,7 +122,7 @@ class MinitaurExtendedEnv(MinitaurReactiveEnv):
leg_model = []
if self._include_leg_model:
raw_motor_angles = self.minitaur.GetMotorAngles()
leg_model = self._convert_to_leg_model(raw_motor_angles)
leg_model = self.convert_to_leg_model(raw_motor_angles)
observation_list = (
[parent_observation] + history_states + history_actions +
@@ -185,7 +185,7 @@ class MinitaurExtendedEnv(MinitaurReactiveEnv):
if self._never_terminate:
return False
leg_model = self._convert_to_leg_model(self.minitaur.GetMotorAngles())
leg_model = self.convert_to_leg_model(self.minitaur.GetMotorAngles())
swing0 = leg_model[0]
swing1 = leg_model[2]
maximum_swing_angle = 0.8

3
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -55,6 +55,7 @@ public:
: m_userPersistentData(0),
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_prevRHS(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
@@ -79,6 +80,7 @@ public:
m_userPersistentData(0),
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_prevRHS(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
@@ -114,6 +116,7 @@ public:
int m_contactPointFlags;
btScalar m_appliedImpulse;
btScalar m_prevRHS;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;

3
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -325,6 +325,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
{
const btManifoldPoint& pt = manifold->getContactPoint(i);
dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
@@ -371,6 +372,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
@@ -416,6 +418,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);

7
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -173,6 +173,7 @@ public:
//get rid of duplicated userPersistentData pointer
m_pointCache[lastUsedIndex].m_userPersistentData = 0;
m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
m_pointCache[lastUsedIndex].m_prevRHS = 0.f;
m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
@@ -195,6 +196,7 @@ public:
#ifdef MAINTAIN_PERSISTENCY
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS;
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
@@ -223,6 +225,7 @@ public:
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_prevRHS = prevRHS;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
}
@@ -276,7 +279,8 @@ struct btPersistentManifoldDoubleData
btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
double m_pointCacheDistance[4];
double m_pointCacheAppliedImpulse[4];
double m_pointCacheCombinedFriction[4];
double m_pointCachePrevRHS[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedRollingFriction[4];
double m_pointCacheCombinedSpinningFriction[4];
double m_pointCacheCombinedRestitution[4];
@@ -322,6 +326,7 @@ struct btPersistentManifoldFloatData
btVector3FloatData m_pointCacheLateralFrictionDir2[4];
float m_pointCacheDistance[4];
float m_pointCacheAppliedImpulse[4];
float m_pointCachePrevRHS[4];
float m_pointCacheCombinedFriction[4];
float m_pointCacheCombinedRollingFriction[4];
float m_pointCacheCombinedSpinningFriction[4];

14
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -30,7 +30,8 @@ enum btSolverMode
SOLVER_SIMD = 256,
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048,
SOLVER_USE_ARTICULATED_WARMSTARTING = 4096,
};
struct btContactSolverInfoData
@@ -54,7 +55,7 @@ struct btContactSolverInfoData
btScalar m_splitImpulseTurnErp;
btScalar m_linearSlop;
btScalar m_warmstartingFactor;
btScalar m_articulatedWarmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
@@ -89,6 +90,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = btScalar(0.0);
m_warmstartingFactor = btScalar(0.85);
m_articulatedWarmstartingFactor = btScalar(0.85);
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2; //unused as of 2.81
@@ -120,6 +122,7 @@ struct btContactSolverInfoDoubleData
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_articulatedWarmstartingFactor;
double m_maxGyroscopicForce; ///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
@@ -150,16 +153,17 @@ struct btContactSolverInfoFloatData
float m_linearSlop;
float m_warmstartingFactor;
float m_articulatedWarmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
#endif //BT_CONTACT_SOLVER_INFO

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -1436,8 +1436,6 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
// Fill padding with zeros to appease msan.
memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
#ifdef BT_USE_DOUBLE_PRECISION
const char* structType = "btDynamicsWorldDoubleData";

34
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -342,40 +342,6 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
solverConstraint.m_friction = 0.f; //cp.m_combinedFriction;
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;

176
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -22,6 +22,8 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "LinearMath/btQuickprof.h"
#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
#include "LinearMath/btScalar.h"
btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
@@ -491,11 +493,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
return deltaVel;
}
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("setupMultiBodyContactConstraint");
btVector3 rel_pos1;
@@ -781,48 +779,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
}
///warm starting (or zero if disabled)
//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
if (/* DISABLES CODE */ (0)) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
}
else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
}
else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
}
}
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
@@ -874,6 +830,54 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
}
if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
{
if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
if (solverConstraint.m_appliedImpulse < 0)
solverConstraint.m_appliedImpulse = 0;
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
}
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
}
else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
}
else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
}
}
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
}
void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
@@ -1130,7 +1134,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
}
}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
@@ -1161,7 +1165,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
solverConstraint.m_originalContactPoint = &cp;
setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -1297,7 +1301,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
solverConstraint.m_originalContactPoint = &cp;
bool isFriction = false;
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction);
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -1371,13 +1375,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
{
applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
@@ -1388,26 +1392,27 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
}
else
{
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
//todo:
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
}
#endif //ENABLE_FRICTION
}
else
{
// Reset quantities related to warmstart as 0.
cp.m_appliedImpulse = 0;
cp.m_prevRHS = 0;
}
}
}
void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
{
//btPersistentManifold* manifold = 0;
for (int i = 0; i < numManifolds; i++)
{
btPersistentManifold* manifold = manifoldPtr[i];
@@ -1434,6 +1439,51 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
}
// Warmstart for noncontact constraints
if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
{
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
{
btMultiBodySolverConstraint& solverConstraint =
m_multiBodyNonContactConstraints[i];
solverConstraint.m_appliedImpulse =
solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
infoGlobal.m_articulatedWarmstartingFactor;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
int ndofA = multiBodyA->getNumDofs() + 6;
btScalar* deltaV =
&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
btScalar impulse = solverConstraint.m_appliedImpulse;
multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
}
if (multiBodyB)
{
int ndofB = multiBodyB->getNumDofs() + 6;
btScalar* deltaV =
&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
btScalar impulse = solverConstraint.m_appliedImpulse;
multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
}
}
}
}
else
{
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
{
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
solverConstraint.m_appliedImpulse = 0;
}
}
}
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
@@ -1556,7 +1606,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
}
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
BT_PROFILE("warm starting write back");
for (int j = 0; j < numPoolConstraints; j++)
@@ -1565,6 +1615,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
pt->m_prevRHS = solverConstraint.m_rhs;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
@@ -1576,9 +1627,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
pt->m_appliedImpulseLateral2 = 0;
}
}
//do a callback here?
}
#if 0
//multibody joint feedback
{

7
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -49,7 +49,7 @@ protected:
void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
@@ -66,7 +66,9 @@ protected:
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
const btScalar& appliedImpulse,
btManifoldPoint& cp,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
@@ -82,7 +84,6 @@ protected:
void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);

72
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -14,7 +14,6 @@
*/
#include "btDeformableContactConstraint.h"
/* ================ Deformable Node Anchor =================== */
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
: m_anchor(&a)
@@ -216,7 +215,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
btVector3 impulse = m_contact->m_c0 * vr;
const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
m_total_normal_dv -= impulse_normal * m_contact->m_c2;
@@ -236,13 +234,13 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
m_static = false;
if (m_total_tangent_dv.norm() < SIMD_EPSILON)
if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
{
m_total_tangent_dv = btVector3(0,0,0);
}
else
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_c3;
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
}
impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
}
@@ -255,7 +253,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
impulse = impulse_normal + impulse_tangent;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
@@ -361,12 +358,32 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
if (face->m_n[0]->m_im > 0)
face->m_n[0]->m_v -= dv * contact->m_weights[0];
if (face->m_n[1]->m_im > 0)
face->m_n[1]->m_v -= dv * contact->m_weights[1];
if (face->m_n[2]->m_im > 0)
face->m_n[2]->m_v -= dv * contact->m_weights[2];
btVector3& v0 = face->m_n[0]->m_v;
btVector3& v1 = face->m_n[1]->m_v;
btVector3& v2 = face->m_n[2]->m_v;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
v0 -= dv * contact->m_weights[0];
if (im1 > 0)
v1 -= dv * contact->m_weights[1];
if (im2 > 0)
v2 -= dv * contact->m_weights[2];
// apply strain limiting to prevent undamped modes
btScalar m01 = (btScalar(1)/(im0 + im1));
btScalar m02 = (btScalar(1)/(im0 + im2));
btScalar m12 = (btScalar(1)/(im1 + im2));
btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
v0 += dv0;
v1 += dv1;
v2 += dv2;
}
/* ================ Face vs. Node =================== */
@@ -449,13 +466,13 @@ btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
m_static = false;
if (m_total_tangent_dv.norm() < SIMD_EPSILON)
if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
{
m_total_tangent_dv = btVector3(0,0,0);
}
else
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction;
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
}
impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
}
@@ -482,16 +499,33 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls
}
btSoftBody::Face* face = contact->m_face;
if (face->m_n[0]->m_im > 0)
btVector3& v0 = face->m_n[0]->m_v;
btVector3& v1 = face->m_n[1]->m_v;
btVector3& v2 = face->m_n[2]->m_v;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
{
face->m_n[0]->m_v -= dvb * contact->m_weights[0];
v0 -= dvb * contact->m_weights[0];
}
if (face->m_n[1]->m_im > 0)
if (im1 > 0)
{
face->m_n[1]->m_v -= dvb * contact->m_weights[1];
v1 -= dvb * contact->m_weights[1];
}
if (face->m_n[2]->m_im > 0)
if (im2 > 0)
{
face->m_n[2]->m_v -= dvb * contact->m_weights[2];
v2 -= dvb * contact->m_weights[2];
}
// todo: Face node constraints needs more work
// btScalar m01 = (btScalar(1)/(im0 + im1));
// btScalar m02 = (btScalar(1)/(im0 + im2));
// btScalar m12 = (btScalar(1)/(im1 + im2));
//
// btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
// btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
// btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
// v0 += dv0;
// v1 += dv1;
// v2 += dv2;
}

36
src/BulletSoftBody/btDeformableNeoHookeanForce.h
View File

@@ -176,24 +176,30 @@ public:
btMatrix3x3 P;
firstPiola(psb->m_tetraScratches[j],P);
#if USE_SVD
btMatrix3x3 U, V;
btVector3 sigma;
singularValueDecomposition(P, U, sigma, V);
if (max_p > 0)
{
sigma[0] = btMin(sigma[0], max_p);
sigma[1] = btMin(sigma[1], max_p);
sigma[2] = btMin(sigma[2], max_p);
sigma[0] = btMax(sigma[0], -max_p);
sigma[1] = btMax(sigma[1], -max_p);
sigma[2] = btMax(sigma[2], -max_p);
// 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();
}
}
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();

11
src/BulletSoftBody/btSoftBody.h
View File

@@ -20,6 +20,7 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btVector3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletCollision/CollisionShapes/btConcaveShape.h"
@@ -973,6 +974,16 @@ public:
/* Return the volume */
btScalar getVolume() const;
/* Cluster count */
btVector3 getCenterOfMass() const
{
btVector3 com(0, 0, 0);
for (int i = 0; i < m_nodes.size(); i++)
{
com += (m_nodes[i].m_x * this->getMass(i));
}
com /= this->getTotalMass();
return com;
}
int clusterCount() const;
/* Cluster center of mass */
static btVector3 clusterCom(const Cluster* cluster);

21
src/LinearMath/btImplicitQRSVD.h
View File

@@ -571,17 +571,18 @@ inline void singularValueDecomposition(
*/
inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
{
btScalar d = (btScalar)0.5 * (a1 - a2);
btScalar bs = b1 * b1;
btScalar val = d * d + bs;
if (val>SIMD_EPSILON)
{
btScalar denom = btFabs(d) + btSqrt(val);
btScalar d = (btScalar)0.5 * (a1 - a2);
btScalar bs = b1 * b1;
btScalar val = d * d + bs;
if (val>SIMD_EPSILON)
{
btScalar denom = btFabs(d) + btSqrt(val);
btScalar mu = a2 - copySign(bs / (denom), d);
// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
return mu;
}
btScalar mu = a2 - copySign(bs / (denom), d);
// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
return mu;
}
return a2;
}
/**

12
src/LinearMath/btScalar.h
View File

@@ -25,13 +25,23 @@ subject to the following restrictions:
#include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 288
#define BT_BULLET_VERSION 289
inline int btGetVersion()
{
return BT_BULLET_VERSION;
}
inline int btIsDoublePrecision()
{
#ifdef BT_USE_DOUBLE_PRECISION
return true;
#else
return false;
#endif
}
// The following macro "BT_NOT_EMPTY_FILE" can be put into a file
// in order suppress the MS Visual C++ Linker warning 4221
//

1198
src/LinearMath/btSerializer.cpp
View File

File diff suppressed because it is too large Load Diff

2
src/LinearMath/btSerializer.h
View File

@@ -481,7 +481,7 @@ public:
buffer[9] = '2';
buffer[10] = '8';
buffer[11] = '8';
buffer[11] = '9';
}
virtual void startSerialization()

1197
src/LinearMath/btSerializer64.cpp
View File

File diff suppressed because it is too large Load Diff