mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-05 15:21:06 +00:00
bump up to Bullet version 2.89 and update serialization structures
This commit is contained in:
Erwin Coumans
parent
938ac51da7
commit
5a9b862ef5
File diff suppressed because it is too large
Load Diff
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -9338,6 +9338,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;
|
||||
|
||||
@ -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,
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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]);
|
||||
@ -444,4 +447,4 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
|
||||
pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
|
||||
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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];
|
||||
|
||||
@ -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
|
||||
|
||||
@ -1436,9 +1436,7 @@ 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";
|
||||
#else //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
//
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -481,7 +481,7 @@ public:
|
||||
|
||||
buffer[9] = '2';
|
||||
buffer[10] = '8';
|
||||
buffer[11] = '8';
|
||||
buffer[11] = '9';
|
||||
}
|
||||
|
||||
virtual void startSerialization()
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user