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

work on pybullet/C-API createMultiBody (still preliminary, only sphere/box collision shapes, no links/hierarchies yet, soon)

Browse Source 
pybullet/C-API, expose linear/angular damping
fix some warnings (param name needs to be same in .h and .cpp)
fix potential startup threading issue (args were deleted in main thread while still possibly use in child thread)
fix for spinning/rolling friction in case of mixing maximal and reduced coordinate btMultiBody+btRigidBody
This commit is contained in:
Erwin Coumans
2017-06-04 22:04:16 -07:00
parent b23cb1dd2c
commit a7aed37632
12 changed files with 183 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -48,7 +48,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ);
virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const;
virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,

36
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -877,6 +877,17 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass
return -2;
}
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
command->m_updateFlags |= MULT_BODY_USE_MAXIMAL_COORDINATES;
}
}
int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
@@ -1269,6 +1280,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId;
break;
}
case CMD_CREATE_MULTI_BODY_COMPLETED:
{
bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
break;
}
default:
{
b3Assert(0);
@@ -1541,9 +1557,29 @@ int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle
command->m_changeDynamicsInfoArgs.m_restitution = restitution;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION;
return 0;
}
int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linearDamping = linearDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING;
return 0;
}
int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
return 0;
}
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

8
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -87,7 +87,8 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa
int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping);
int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
@@ -329,9 +330,10 @@ int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]);
//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle);
//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId);

29
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3530,9 +3530,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED;
if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0)
{
m_data->m_sdfRecentLoadedBodies.clear();
ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data);
bool useMultiBody = false;
bool useMultiBody = true;
if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES)
{
useMultiBody = false;
}
int flags = 0;
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
@@ -4329,6 +4336,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
mb->getBaseCollider()->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getBaseCollider()->setFriction(lateralFriction);
@@ -4395,6 +4411,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (body && body->m_rigidBody)
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
btScalar angDamping = body->m_rigidBody->getAngularDamping();
body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
btScalar linDamping = body->m_rigidBody->getLinearDamping();
body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
body->m_rigidBody->setRestitution(restitution);

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -117,6 +117,8 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION=8,
CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION=16,
CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32,
CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING=64,
CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128,
};
struct ChangeDynamicsInfoArgs
@@ -129,6 +131,8 @@ struct ChangeDynamicsInfoArgs
double m_spinningFriction;
double m_rollingFriction;
double m_restitution;
double m_linearDamping;
double m_angularDamping;
};
struct GetDynamicsInfoArgs
@@ -806,6 +810,7 @@ struct b3CreateVisualShapeArgs
enum eCreateMultiBodyEnum
{
MULTI_BODY_HAS_BASE=1,
MULT_BODY_USE_MAXIMAL_COORDINATES=2,
};
struct b3CreateMultiBodyArgs
{

16
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -93,22 +93,23 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg
class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserInternalData* m_data;
public:
char** m_newargv;
public:
InProcessPhysicsClientSharedMemory(int argc, char* argv[])
{
int newargc = argc+2;
char** newargv = (char**)malloc(sizeof(void*)*newargc);
m_newargv = (char**)malloc(sizeof(void*)*newargc);
for (int i=0;i<argc;i++)
newargv[i] = argv[i];
m_newargv[i] = argv[i];
char* t0 = (char*)"--logtostderr";
char* t1 = (char*)"--start_demo_name=Physics Server";
newargv[argc] = t0;
newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc,newargv);
m_newargv[argc] = t0;
m_newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc,m_newargv);
SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);
free(newargv);
setSharedMemoryInterface(shMem);
}
@@ -116,6 +117,7 @@ public:
{
setSharedMemoryInterface(0);
btShutDownExampleBrowser(m_data);
free(m_newargv);
}
};

2
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -22,7 +22,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
virtual void changeRGBAColor(int bodyUniqueId, int shapeIndex, const double rgbaColor[4]);
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, const double rgbaColor[4]);
virtual void removeVisualShape(class btCollisionObject* colObj);

24
examples/pybullet/examples/createSphereMultiBodies.py
View File

@@ -1,9 +1,13 @@
import pybullet as p
import time
p.connect(p.GUI)
sphereRadius = 0.05
useMaximalCoordinates = 0
p.connect(p.GUI)
p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(fixedTimeStep=1./120.)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
@@ -11,16 +15,16 @@ mass = 1
visualShapeId = -1
for i in range (10):
for j in range (10):
for k in range (10):
for i in range (5):
for j in range (5):
for k in range (5):
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1])
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1])
#p.changeDynamics(sphereUid,-1,spinningFriction=0.1, rollingFriction=0.1)
#p.loadSDF("stadium.sdf")
p.loadURDF("plane.urdf", useMaximalCoordinates=0)
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)

38
examples/pybullet/pybullet.c
View File

@@ -623,12 +623,14 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
double spinningFriction= -1;
double rollingFriction = -1;
double restitution = -1;
double linearDamping = -1;
double angularDamping = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &physicsClientId))
{
return NULL;
}
@@ -661,6 +663,16 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex,rollingFriction);
}
if (linearDamping>=0)
{
b3ChangeDynamicsInfoSetLinearDamping(command,bodyUniqueId, linearDamping);
}
if (angularDamping>=0)
{
b3ChangeDynamicsInfoSetAngularDamping(command,bodyUniqueId,angularDamping);
}
if (restitution>=0)
{
b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution);
@@ -971,6 +983,7 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
int numBodies = 0;
int i;
int bodyIndicesOut[MAX_SDF_BODIES];
int useMaximalCoordinates = -1;
PyObject* pylist = 0;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
@@ -978,8 +991,8 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"sdfFileName", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName, &physicsClientId))
static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &sdfFileName, &useMaximalCoordinates, &physicsClientId))
{
return NULL;
}
@@ -991,6 +1004,11 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
}
commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
if (useMaximalCoordinates>0)
{
b3LoadSdfCommandSetUseMultiBody(commandHandle,0);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SDF_LOADING_COMPLETED)
@@ -4805,15 +4823,17 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
double baseMass = 0;
int baseCollisionShapeIndex=-1;
int baseVisualShapeIndex=-1;
int useMaximalCoordinates = 0;
PyObject* basePosObj=0;
PyObject* baseOrnObj=0;
static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation",
// "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices",
"useMaximalCoordinates","physicsClientId",
NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOi", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex,
&basePosObj, &baseOrnObj,&physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex,
&basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId))
{
return NULL;
}
@@ -4833,11 +4853,15 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
pybullet_internalSetVectord(basePosObj,basePosition);
pybullet_internalSetVector4d(baseOrnObj,baseOrientation);
int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation);
if (useMaximalCoordinates>0)
{
b3CreateMultiBodyUseMaximalCoordinates(commandHandle,useMaximalCoordinates);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED)
{
int uid = b3GetStatusMultiBodyUniqueId(statusHandle);
int uid = b3GetStatusBodyIndex(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}

2
src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
View File

@@ -59,7 +59,7 @@ public:
) = 0;
};
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
protected:
btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands

10
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -2013,10 +2013,10 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
}
mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
// Fill padding with zeros to appease msan.
#ifdef BT_USE_DOUBLE_PRECISION
memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
#endif
// Fill padding with zeros to appease msan.
#ifdef BT_USE_DOUBLE_PRECISION
memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
#endif
return btMultiBodyDataName;
}

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

@@ -665,12 +665,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis0 = constraintNormal;
btVector3 torqueAxis0 = -constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = btVector3(0,0,0);
} else
{
btVector3 torqueAxis0 = constraintNormal;
btVector3 torqueAxis0 = -constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = btVector3(0,0,0);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -708,21 +708,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis1 = constraintNormal;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
} else
{
btVector3 torqueAxis1 = constraintNormal;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
@@ -745,8 +744,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
{
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + constraintNormal.dot(vec);
btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
}
}
if (multiBodyB)
@@ -765,8 +764,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
{
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + constraintNormal.dot(vec);
btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
}
}
@@ -808,7 +807,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
{
if (rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0));
}
}
if (multiBodyB)
@@ -822,7 +824,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
{
if (rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0));
}
}
@@ -844,12 +849,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
{
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
btScalar velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
if (penetration>0)
{
velocityError -= penetration / infoGlobal.m_timeStep;
}
btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
@@ -1021,6 +1023,33 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
cp.m_lateralFrictionDir1.normalize();
cp.m_lateralFrictionDir2.normalize();
if (rollingFriction > 0 )
{
if (cp.m_combinedSpinningFriction>0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
}
if (cp.m_combinedRollingFriction>0)
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (cp.m_lateralFrictionDir1.length()>0.001)
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
if (cp.m_lateralFrictionDir2.length()>0.001)
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
}
rollingFriction--;
}
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{/*
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -1045,26 +1074,12 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
} else
*/
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
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);
if (rollingFriction > 0 )
{
if (cp.m_combinedSpinningFriction>0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
}
if (cp.m_combinedRollingFriction>0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
}
rollingFriction--;
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{