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

Merge branch 'master' into pr-fix-thread-index

Browse Source
This commit is contained in:
lunkhound
2017-06-05 00:33:02 -07:00
committed by GitHub
parent 0fc3ce69e9 69642805ff
commit 6bf3d4e08e
23 changed files with 1200 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

@@ -237,7 +237,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
if (params.m_clampToIntegers)
{
pSlider->SetNotchCount( int( params.m_maxVal - params.m_minVal ) );
pSlider->SetClampToNotches( params.m_clampToNotches );
pSlider->SetClampToNotches( true );
}
else
{

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,

8
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -294,7 +294,6 @@ std::string BulletURDFImporter::getJointName(int linkIndex) const
void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
//todo(erwincoumans)
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
@@ -414,6 +413,13 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
}
void BulletURDFImporter::setRootTransformInWorld(const btTransform& rootTransformInWorld)
{
m_data->m_urdfParser.getModel().m_rootTransformInWorld = rootTransformInWorld ;
}
bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld;

1
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -55,6 +55,7 @@ public:
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;

3
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -64,7 +64,8 @@ public:
};
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld){}
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}

3
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -282,12 +282,11 @@ public:
int getNumModels() const
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
if (m_parseSDF)
{
return m_sdfModels.size();
}
return 0;
return 1;
}
void activateModel(int modelIndex);

2
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@@ -244,7 +244,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector3 color(0.0,0.0,0.5);
btVector4 color(0.0,0.0,0.5,1);
guiHelper->createCollisionObjectGraphicsObject(col,color);
// col->setFriction(friction);

7
examples/OpenGLWindow/SimpleOpenGL2App.cpp
View File

@@ -481,7 +481,8 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world
void SimpleOpenGL2App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4])
{
b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5);
cubeExtents[m_data->m_upAxis] = 0;
double halfHeight=0.1;
cubeExtents[m_data->m_upAxis] = halfHeight;
int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]);
b3Quaternion orn(0,0,0,1);
@@ -501,10 +502,10 @@ void SimpleOpenGL2App::registerGrid(int cells_x, int cells_z, float color0[4], f
}
if (this->m_data->m_upAxis==1)
{
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f);
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, -halfHeight, (j + 0.5f) - cells_z * 0.5f);
} else
{
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f );
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,-halfHeight );
}
m_renderer->registerGraphicsInstance(cubeId,center,orn,color,scaling);
}

8
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -704,9 +704,9 @@ int SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, f
void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4])
{
b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5);
cubeExtents[m_data->m_upAxis] = 0;
double halfHeight=0.1;
cubeExtents[m_data->m_upAxis] = halfHeight;
int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]);
b3Quaternion orn(0,0,0,1);
b3Vector3 center=b3MakeVector3(0,0,0,1);
b3Vector3 scaling=b3MakeVector3(1,1,1,1);
@@ -724,10 +724,10 @@ void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], f
}
if (this->m_data->m_upAxis==1)
{
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f);
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, -halfHeight, (j + 0.5f) - cells_z * 0.5f);
} else
{
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f );
center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,-halfHeight );
}
m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling);
}

235
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -691,6 +691,216 @@ int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
return 0;
}
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_COLLISION_SHAPE;
command->m_updateFlags =0;
command->m_createCollisionShapeArgs.m_numCollisionShapes = 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
}
}
}
void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2];
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
}
}
}
void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
{
if (shapeIndex<command->m_createCollisionShapeArgs.m_numCollisionShapes)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3];
}
}
}
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED);
if (status && status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
return status->m_createCollisionShapeResultArgs.m_collisionShapeUniqueId;
}
return -1;
}
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_VISUAL_SHAPE;
command->m_updateFlags =0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED);
if (status && status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
{
return status->m_createVisualShapeResultArgs.m_visualShapeUniqueId;
}
return -1;
}
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_MULTI_BODY;
command->m_updateFlags =0;
command->m_createMultiBodyArgs.m_bodyName[0] = 0;
command->m_createMultiBodyArgs.m_baseLinkIndex = -1;
command->m_createMultiBodyArgs.m_numLinks = 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
int numLinks = command->m_createMultiBodyArgs.m_numLinks;
if (numLinks<MAX_CREATE_MULTI_BODY_LINKS)
{
int baseLinkIndex = numLinks;
command->m_updateFlags |=MULTI_BODY_HAS_BASE;
command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex;
command->m_createMultiBodyArgs.m_baseWorldPosition[0]=basePosition[0];
command->m_createMultiBodyArgs.m_baseWorldPosition[1]=basePosition[1];
command->m_createMultiBodyArgs.m_baseWorldPosition[2]=basePosition[2];
command->m_createMultiBodyArgs.m_baseWorldOrientation[0]=baseOrientation[0];
command->m_createMultiBodyArgs.m_baseWorldOrientation[1]=baseOrientation[1];
command->m_createMultiBodyArgs.m_baseWorldOrientation[2]=baseOrientation[2];
command->m_createMultiBodyArgs.m_baseWorldOrientation[3]=baseOrientation[3];
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass;
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass;
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+2] = mass;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+0] = 0;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+1] = 0;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+2] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+0] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+1] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+2] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+3] = 1;
command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique;
command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId;
command->m_createMultiBodyArgs.m_linkMasses[command->m_createMultiBodyArgs.m_numLinks] = mass;
command->m_createMultiBodyArgs.m_numLinks++;
}
return numLinks;
}
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)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED);
if (status && status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED)
{
return status->m_createMultiBodyResultArgs.m_bodyUniqueId;
}
return -1;
}
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1070,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);
@@ -1342,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;

26
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);
@@ -313,6 +314,29 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
///the creation of collision shapes and rigid bodies etc is likely going to change,
///but good to have a b3CreateBoxShapeCommandInit for now
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]);
void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]);
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId);
///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
///after that, you can optionally adjust the initial position, orientation and size
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);

24
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -442,6 +442,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_CREATE_MULTI_BODY_COMPLETED:
case CMD_URDF_LOADING_COMPLETED:
{
B3_PROFILE("CMD_URDF_LOADING_COMPLETED");
@@ -1056,6 +1058,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Request dynamics info failed");
break;
}
case CMD_CREATE_COLLISION_SHAPE_FAILED:
{
b3Warning("Request createCollisionShape failed");
break;
}
case CMD_CREATE_COLLISION_SHAPE_COMPLETED:
case CMD_CREATE_VISUAL_SHAPE_COMPLETED:
{
break;
}
case CMD_CREATE_MULTI_BODY_FAILED:
{
b3Warning("Request createMultiBody failed");
break;
}
case CMD_CREATE_VISUAL_SHAPE_FAILED:
{
b3Warning("Request createVisualShape failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);

28
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -835,6 +835,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
break;
}
case CMD_CREATE_MULTI_BODY_COMPLETED:
case CMD_URDF_LOADING_COMPLETED:
{
@@ -881,6 +882,33 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
b3Warning("createConstraint failed");
break;
}
case CMD_CREATE_COLLISION_SHAPE_FAILED:
{
b3Warning("createCollisionShape failed");
break;
}
case CMD_CREATE_COLLISION_SHAPE_COMPLETED:
{
break;
}
case CMD_CREATE_VISUAL_SHAPE_FAILED:
{
b3Warning("createVisualShape failed");
break;
}
case CMD_CREATE_VISUAL_SHAPE_COMPLETED:
{
break;
}
case CMD_CREATE_MULTI_BODY_FAILED:
{
b3Warning("createMultiBody failed");
break;
}
default:
{
//b3Warning("Unknown server status type");

462
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -124,7 +124,14 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
}
};
struct InteralCollisionShapeData
{
btCollisionShape* m_collisionShape;
void clear()
{
m_collisionShape=0;
}
};
struct InteralBodyData
{
@@ -171,7 +178,7 @@ struct InteralUserConstraintData
};
typedef b3PoolBodyHandle<InteralBodyData> InternalBodyHandle;
typedef b3PoolBodyHandle<InteralCollisionShapeData> InternalCollisionShapeHandle;
class btCommandChunk
{
@@ -1133,7 +1140,7 @@ struct PhysicsServerCommandProcessorInternalData
{
///handle management
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
bool m_allowRealTimeSimulation;
@@ -1244,6 +1251,9 @@ struct PhysicsServerCommandProcessorInternalData
m_bodyHandles.exitHandles();
m_bodyHandles.initHandles();
m_userCollisionShapeHandles.exitHandles();
m_userCollisionShapeHandles.initHandles();
#if 0
btAlignedObjectArray<int> bla;
@@ -1490,6 +1500,211 @@ void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
}
struct ProgrammaticUrdfInterface : public URDFImporterInterface
{
int m_bodyUniqueId;
const b3CreateMultiBodyArgs& m_createBodyArgs;
mutable b3AlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
PhysicsServerCommandProcessorInternalData* m_data;
ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data)
:m_bodyUniqueId(-1),
m_createBodyArgs(bodyArgs),
m_data(data)
{
}
virtual ~ProgrammaticUrdfInterface()
{
}
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
{
b3Assert(0);
return false;
}
virtual const char* getPathPrefix()
{
return "";
}
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const
{
return m_createBodyArgs.m_baseLinkIndex;
}
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const
{
return "link";
}
//various derived class in internal source code break with new pure virtual methods, so provide some default implementation
virtual std::string getBodyName() const
{
return m_createBodyArgs.m_bodyName;
}
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const
{
b3Assert(0);
return false;
}
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
{
return false;
}
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const
{
return 0;
}
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
{
return false;
}
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const
{
b3Assert(0);
return false;
}
virtual std::string getJointName(int linkIndex) const
{
b3Assert(0);
return "joint";
}
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
if (urdfLinkIndex>=0 && urdfLinkIndex < m_createBodyArgs.m_numLinks)
{
mass = m_createBodyArgs.m_linkMasses[urdfLinkIndex];
localInertiaDiagonal.setValue(
m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+0],
m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+1],
m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+2]);
inertialFrame.setOrigin(btVector3(
m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+0],
m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+1],
m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+2]));
inertialFrame.setRotation(btQuaternion(
m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+0],
m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+1],
m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+2],
m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+3]));
} else
{
mass = 0;
localInertiaDiagonal.setValue(0,0,0);
inertialFrame.setIdentity();
}
}
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
}
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
return false;
};
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
{
//backwards compatibility for custom file importers
jointMaxForce = 0;
jointMaxVelocity = 0;
return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
};
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
rootTransformInWorld.setOrigin(btVector3(
m_createBodyArgs.m_baseWorldPosition[0],
m_createBodyArgs.m_baseWorldPosition[1],
m_createBodyArgs.m_baseWorldPosition[2]));
rootTransformInWorld.setRotation(btQuaternion(
m_createBodyArgs.m_baseWorldOrientation[0],
m_createBodyArgs.m_baseWorldOrientation[1],
m_createBodyArgs.m_baseWorldOrientation[2],
m_createBodyArgs.m_baseWorldOrientation[3]));
return true;
}
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld)
{
b3Assert(0);
}
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
{
return -1;
}
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
{
}
virtual void setBodyUniqueId(int bodyId)
{
m_bodyUniqueId = bodyId;
}
virtual int getBodyUniqueId() const
{
return m_bodyUniqueId;
}
//default implementation for backward compatibility
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compound = new btCompoundShape();
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex];
if (colShapeUniqueId>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
{
btTransform childTrans;
childTrans.setIdentity();
compound->addChildShape(localInertiaFrame.inverse()*childTrans,handle->m_collisionShape);
}
}
m_allocatedCollisionShapes.push_back(compound);
return compound;
}
virtual int getNumAllocatedCollisionShapes() const
{
return m_allocatedCollisionShapes.size();
}
virtual class btCollisionShape* getAllocatedCollisionShape(int index)
{
return m_allocatedCollisionShapes[index];
}
virtual int getNumModels() const
{
return 1;
}
virtual void activateModel(int /*modelIndex*/)
{
}
};
void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
@@ -1829,7 +2044,22 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
createJointMotors(mb);
#ifdef B3_ENABLE_TINY_AUDIO
{
SDFAudioSource audioSource;
int urdfRootLink = u2b.getRootLinkIndex();//LinkIndex = creation.m_mb2urdfLink[-1];
if (u2b.getLinkAudioSource(urdfRootLink,audioSource))
{
int flags = mb->getBaseCollider()->getCollisionFlags();
mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
if (audioSource.m_userIndex>=0)
{
bodyHandle->m_audioSources.insert(-1, audioSource);
}
}
}
#endif
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
@@ -1853,14 +2083,33 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
m_data->m_strings.push_back(jointName);
mb->getLink(i).m_jointName = jointName->c_str();
#ifdef B3_ENABLE_TINY_AUDIO
{
SDFAudioSource audioSource;
int urdfLinkIndex = creation.m_mb2urdfLink[link];
if (u2b.getLinkAudioSource(urdfLinkIndex,audioSource))
{
int flags = mb->getLink(link).m_collider->getCollisionFlags();
mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
if (audioSource.m_userIndex>=0)
{
bodyHandle->m_audioSources.insert(link, audioSource);
}
}
}
#endif
}
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_data->m_strings.push_back(baseName);
mb->setBaseName(baseName->c_str());
} else
{
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
//b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
bodyHandle->m_rigidBody = rb;
rb->setUserIndex2(bodyUniqueId);
m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
}
}
@@ -1945,6 +2194,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags)
{
m_data->m_sdfRecentLoadedBodies.clear();
*bodyUniqueIdPtr = -1;
BT_PROFILE("loadURDF");
btAssert(m_data->m_dynamicsWorld);
if (!m_data->m_dynamicsWorld)
@@ -1962,6 +2214,25 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (loadOk)
{
#if 1
btTransform rootTrans;
rootTrans.setOrigin(pos);
rootTrans.setRotation(orn);
u2b.setRootTransformInWorld(rootTrans);
bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
if (ok)
{
if (m_data->m_sdfRecentLoadedBodies.size()==1)
{
*bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0];
}
m_data->m_sdfRecentLoadedBodies.clear();
}
return ok;
#else
//get a body index
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
if (bodyUniqueIdPtr)
@@ -2132,9 +2403,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
return true;
}
}
#endif
}
return false;
}
@@ -3155,6 +3426,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_CREATE_COLLISION_SHAPE:
{
hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
btCollisionShape* shape = 0;
btCompoundShape* compound = 0;
if (clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes>1)
{
compound = worldImporter->createCompoundShape();
}
for (int i=0;i<clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes;i++)
{
btTransform childTransform;
childTransform.setIdentity();
if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_hasChildTransform)
{
childTransform.setOrigin(btVector3(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childPosition[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childPosition[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childPosition[2]));
childTransform.setRotation(btQuaternion(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[2],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[3]
));
if (compound==0)
{
compound = worldImporter->createCompoundShape();
}
}
switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type)
{
case GEOM_SPHERE:
{
double radius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
break;
}
case GEOM_BOX:
{
//double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createBoxShape(btVector3(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]));
if (compound)
{
compound->addChildShape(childTransform,shape);
}
break;
}
}
}
#if 0
shape = worldImporter->createCylinderShapeX(radius,height);
shape = worldImporter->createCylinderShapeY(radius,height);
shape = worldImporter->createCylinderShapeZ(radius,height);
shape = worldImporter->createCapsuleShapeX(radius,height);
shape = worldImporter->createCapsuleShapeY(radius,height);
shape = worldImporter->createCapsuleShapeZ(radius,height);
shape = worldImporter->createBoxShape(halfExtents);
#endif
if (compound && compound->getNumChildShapes())
{
shape = compound;
}
if (shape)
{
int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle();
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid);
handle->m_collisionShape = shape;
serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid;
m_data->m_worldImporters.push_back(worldImporter);
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;
} else
{
delete worldImporter;
}
break;
}
case CMD_CREATE_VISUAL_SHAPE:
{
hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
break;
}
case CMD_CREATE_MULTI_BODY:
{
hasStatus = true;
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 = true;
if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES)
{
useMultiBody = false;
}
int flags = 0;
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
if (ok)
{
int bodyUniqueId = -1;
if (m_data->m_sdfRecentLoadedBodies.size()==1)
{
bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0];
}
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId>=0)
{
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
}
}
//ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
}
break;
}
case CMD_LOAD_URDF:
{
BT_PROFILE("CMD_LOAD_URDF");
@@ -3193,13 +3612,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
initialPos,initialOrn,
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags);
if (completedOk)
if (completedOk && bodyUniqueId>=0)
{
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
if (m_data->m_urdfLinkNameMapper.size())
{
@@ -3914,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);
@@ -3980,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);
@@ -6409,8 +6851,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_bodyHandles.exitHandles();
m_data->m_bodyHandles.initHandles();
m_data->m_userCollisionShapeHandles.exitHandles();
m_data->m_userCollisionShapeHandles.initHandles();
}

100
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
@@ -761,6 +765,96 @@ struct ConfigureOpenGLVisualizerRequest
int m_setEnabled;
};
enum
{
URDF_GEOM_HAS_RADIUS = 1,
};
struct b3CreateCollisionShape
{
int m_type;//see UrdfGeomTypes
int m_hasChildTransform;
double m_childPosition[3];
double m_childOrientation[4];
double m_sphereRadius;
double m_boxHalfExtents[3];
double m_capsuleRadius;
double m_capsuleHeight;
int m_hasFromTo;
double m_capsuleFrom[3];
double m_capsuleTo[3];
double m_planeNormal[3];
int m_meshFileType;
char m_meshFileName[1024];
double m_meshScale;
};
#define MAX_COMPOUND_COLLISION_SHAPES 128
struct b3CreateCollisionShapeArgs
{
int m_numCollisionShapes;
b3CreateCollisionShape m_shapes[MAX_COMPOUND_COLLISION_SHAPES];
};
struct b3CreateVisualShapeArgs
{
int m_visualShapeUniqueId;
};
#define MAX_CREATE_MULTI_BODY_LINKS 128
enum eCreateMultiBodyEnum
{
MULTI_BODY_HAS_BASE=1,
MULT_BODY_USE_MAXIMAL_COORDINATES=2,
};
struct b3CreateMultiBodyArgs
{
char m_bodyName[1024];
int m_baseLinkIndex;
double m_baseWorldPosition[3];
double m_baseWorldOrientation[4];
int m_numLinks;
double m_linkMasses[MAX_CREATE_MULTI_BODY_LINKS];
double m_linkInertias[MAX_CREATE_MULTI_BODY_LINKS*3];
double m_linkInertialFramePositions[MAX_CREATE_MULTI_BODY_LINKS*3];
double m_linkInertialFrameOrientations[MAX_CREATE_MULTI_BODY_LINKS*4];
int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS];
int m_linkCollisionShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS];
int m_linkVisualShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS];
#if 0
std::string m_name;
std::string m_sourceFile;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
#endif
};
struct b3CreateCollisionShapeResultArgs
{
int m_collisionShapeUniqueId;
};
struct b3CreateVisualShapeResultArgs
{
int m_visualShapeUniqueId;
};
struct b3CreateMultiBodyResultArgs
{
int m_bodyUniqueId;
};
struct SharedMemoryCommand
{
int m_type;
@@ -808,6 +902,9 @@ struct SharedMemoryCommand
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
struct b3ObjectArgs m_removeObjectArgs;
struct b3Profile m_profile;
struct b3CreateCollisionShapeArgs m_createCollisionShapeArgs;
struct b3CreateVisualShapeArgs m_createVisualShapeArgs;
struct b3CreateMultiBodyArgs m_createMultiBodyArgs;
};
};
@@ -874,6 +971,9 @@ struct SharedMemoryStatus
struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs;
struct b3ObjectArgs m_removeObjectArgs;
struct b3DynamicsInfo m_dynamicsInfo;
struct b3CreateCollisionShapeResultArgs m_createCollisionShapeResultArgs;
struct b3CreateVisualShapeResultArgs m_createVisualShapeResultArgs;
struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs;
};
};

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

24
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,6 +7,7 @@
#define SHARED_MEMORY_MAGIC_NUMBER 201706001
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024
enum EnumSharedMemoryClientCommand
{
CMD_LOAD_SDF,
@@ -60,6 +61,9 @@ enum EnumSharedMemoryClientCommand
CMD_CHANGE_DYNAMICS_INFO,
CMD_GET_DYNAMICS_INFO,
CMD_PROFILE_TIMING,
CMD_CREATE_COLLISION_SHAPE,
CMD_CREATE_VISUAL_SHAPE,
CMD_CREATE_MULTI_BODY,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -145,6 +149,12 @@ enum EnumSharedMemoryServerStatus
CMD_REMOVE_BODY_FAILED,
CMD_GET_DYNAMICS_INFO_COMPLETED,
CMD_GET_DYNAMICS_INFO_FAILED,
CMD_CREATE_COLLISION_SHAPE_FAILED,
CMD_CREATE_COLLISION_SHAPE_COMPLETED,
CMD_CREATE_VISUAL_SHAPE_FAILED,
CMD_CREATE_VISUAL_SHAPE_COMPLETED,
CMD_CREATE_MULTI_BODY_FAILED,
CMD_CREATE_MULTI_BODY_COMPLETED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -204,6 +214,7 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3UserConstraint
{
int m_parentBodyIndex;
@@ -537,4 +548,17 @@ enum eURDF_Flags
URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
};
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
{
GEOM_SPHERE=2,
GEOM_BOX,
GEOM_CYLINDER,
GEOM_MESH,
GEOM_PLANE,
GEOM_CAPSULE, //non-standard URDF?
GEOM_UNKNOWN,
};
#endif//SHARED_MEMORY_PUBLIC_H

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);

34
examples/pybullet/examples/createSphereMultiBodies.py Normal file
View File

@@ -0,0 +1,34 @@
import pybullet as p
import time
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])
mass = 1
visualShapeId = -1
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],useMaximalCoordinates=useMaximalCoordinates)
else:
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)
while (1):
time.sleep(0.01)

197
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)
@@ -4701,6 +4719,158 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
return NULL;
}
static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int shapeType=-1;
double radius=-1;
PyObject* halfExtentsObj=0;
static char* kwlist[] = {"shapeType","radius","halfExtents", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOi", kwlist,
&shapeType, &radius,&halfExtentsObj, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (shapeType>=GEOM_SPHERE)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm);
if (shapeType==GEOM_SPHERE && radius>0)
{
b3CreateCollisionShapeAddSphere(commandHandle,radius);
}
if (shapeType==GEOM_BOX && halfExtentsObj)
{
double halfExtents[3] = {1,1,1};
pybullet_internalSetVectord(halfExtentsObj,halfExtents);
b3CreateCollisionShapeAddBox(commandHandle,halfExtents);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createCollisionShape failed.");
return NULL;
}
#if 0
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle);
#endif
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int test=-1;
static char* kwlist[] = {"test",NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &test,
&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (test>=0)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
{
int uid = b3GetStatusVisualShapeUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createVisualShape failed.");
return NULL;
}
static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
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, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex,
&basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm);
double basePosition[3]={0,0,0};
double baseOrientation[4]={0,0,0,1};
pybullet_internalSetVectord(basePosObj,basePosition);
pybullet_internalSetVector4d(baseOrnObj,baseOrientation);
int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation);
if (useMaximalCoordinates>0)
{
b3CreateMultiBodyUseMaximalCoordinates(commandHandle);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED)
{
int uid = b3GetStatusBodyIndex(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createMultiBody failed.");
return NULL;
}
static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
@@ -6218,6 +6388,16 @@ static PyMethodDef SpamMethods[] = {
{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
"Load multibodies from an MJCF file."},
{"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS,
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
"Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{"createMultiBody", (PyCFunction)pybullet_createMultiBody, METH_VARARGS | METH_KEYWORDS,
"Create a multi body. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."},
@@ -6652,6 +6832,15 @@ initpybullet(void)
PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT);
PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN);
PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);

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))
{