Merge pull request #859 from erwincoumans/master
pybullet loadBullet/saveBullet, allow ExampleBrowser with --opengl2 fallback with z-up axis,
This commit is contained in:
erwincoumans
GitHub
@@ -1,7 +0,0 @@
|
||||
--start_demo_name=R2D2 Grasp
|
||||
--mouse_move_multiplier=0.400000
|
||||
--mouse_wheel_multiplier=0.010000
|
||||
--background_color_red= 0.900000
|
||||
--background_color_green= 0.900000
|
||||
--background_color_blue= 1.000000
|
||||
--fixed_timestep= 0.000000
|
||||
Binary file not shown.
@@ -69,6 +69,12 @@ struct GUIHelperInterface
|
||||
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
|
||||
|
||||
|
||||
|
||||
virtual int addUserDebugText3D( const char* txt, const double posisionXYZ[3], const double textColorRGB[3], double size, double lifeTime)=0;
|
||||
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )=0;
|
||||
virtual void removeUserDebugItem( int debugItemUniqueId)=0;
|
||||
virtual void removeAllUserDebugItems( )=0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -142,6 +148,21 @@ struct DummyGUIHelper : public GUIHelperInterface
|
||||
{
|
||||
}
|
||||
|
||||
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual void removeUserDebugItem( int debugItemUniqueId)
|
||||
{
|
||||
}
|
||||
virtual void removeAllUserDebugItems( )
|
||||
{
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //GUI_HELPER_INTERFACE_H
|
||||
|
||||
@@ -782,7 +782,7 @@ GL_ShapeDrawer::~GL_ShapeDrawer()
|
||||
}
|
||||
}
|
||||
|
||||
void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWorld, int pass)
|
||||
void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWorld, int pass, int cameraUpAxis)
|
||||
{
|
||||
|
||||
btAssert(dynamicsWorld);
|
||||
@@ -849,7 +849,12 @@ void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWo
|
||||
//if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe))
|
||||
int debugMode = 0;//getDebugMode()
|
||||
//btVector3 m_sundirection(-1,-1,-1);
|
||||
|
||||
btVector3 m_sundirection(btVector3(1,-2,1)*1000);
|
||||
if (cameraUpAxis==2)
|
||||
{
|
||||
m_sundirection = btVector3(1,1,-2)*1000;
|
||||
}
|
||||
|
||||
switch(pass)
|
||||
{
|
||||
@@ -861,9 +866,12 @@ void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWo
|
||||
|
||||
}
|
||||
|
||||
void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, bool useShadows)
|
||||
//this GL_ShapeDrawer will be removed, in the meanwhile directly access this global 'useShadoMaps'
|
||||
extern bool useShadowMap;
|
||||
void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, bool useShadows1, int cameraUpAxis)
|
||||
{
|
||||
|
||||
bool useShadows = useShadowMap;
|
||||
GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) };
|
||||
GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0) };
|
||||
GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0 )};
|
||||
@@ -897,7 +905,7 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
|
||||
{
|
||||
glClear(GL_STENCIL_BUFFER_BIT);
|
||||
glEnable(GL_CULL_FACE);
|
||||
drawSceneInternal(dynamicsWorld,0);
|
||||
drawSceneInternal(dynamicsWorld,0, cameraUpAxis);
|
||||
|
||||
glDisable(GL_LIGHTING);
|
||||
glDepthMask(GL_FALSE);
|
||||
@@ -907,10 +915,10 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
|
||||
glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
|
||||
glFrontFace(GL_CCW);
|
||||
glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
|
||||
drawSceneInternal(dynamicsWorld,1);
|
||||
drawSceneInternal(dynamicsWorld,1,cameraUpAxis);
|
||||
glFrontFace(GL_CW);
|
||||
glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
|
||||
drawSceneInternal(dynamicsWorld,1);
|
||||
drawSceneInternal(dynamicsWorld,1,cameraUpAxis);
|
||||
glFrontFace(GL_CCW);
|
||||
|
||||
glPolygonMode(GL_FRONT,GL_FILL);
|
||||
@@ -929,7 +937,7 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
|
||||
glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
|
||||
glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
|
||||
glDisable(GL_LIGHTING);
|
||||
drawSceneInternal(dynamicsWorld,2);
|
||||
drawSceneInternal(dynamicsWorld,2,cameraUpAxis);
|
||||
glEnable(GL_LIGHTING);
|
||||
glDepthFunc(GL_LESS);
|
||||
glDisable(GL_STENCIL_TEST);
|
||||
@@ -938,6 +946,6 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
|
||||
else
|
||||
{
|
||||
glDisable(GL_CULL_FACE);
|
||||
drawSceneInternal(dynamicsWorld,0);
|
||||
drawSceneInternal(dynamicsWorld,0,cameraUpAxis);
|
||||
}
|
||||
}
|
||||
@@ -44,7 +44,7 @@ protected:
|
||||
|
||||
ShapeCache* cache(btConvexShape*);
|
||||
|
||||
virtual void drawSceneInternal(const btDiscreteDynamicsWorld* world, int pass);
|
||||
virtual void drawSceneInternal(const btDiscreteDynamicsWorld* world, int pass, int cameraUpAxis);
|
||||
|
||||
public:
|
||||
GL_ShapeDrawer();
|
||||
@@ -53,7 +53,7 @@ public:
|
||||
|
||||
|
||||
|
||||
virtual void drawScene(const btDiscreteDynamicsWorld* world, bool useShadows);
|
||||
virtual void drawScene(const btDiscreteDynamicsWorld* world, bool useShadows, int cameraUpAxis);
|
||||
|
||||
///drawOpenGL might allocate temporary memoty, stores pointer in shape userpointer
|
||||
virtual void drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax);
|
||||
|
||||
@@ -338,7 +338,7 @@ void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld)
|
||||
if (m_data->m_gl2ShapeDrawer && rbWorld)
|
||||
{
|
||||
m_data->m_gl2ShapeDrawer->enableTexture(true);
|
||||
m_data->m_gl2ShapeDrawer->drawScene(rbWorld,true);
|
||||
m_data->m_gl2ShapeDrawer->drawScene(rbWorld,true, m_data->m_glApp->getUpAxis());
|
||||
}
|
||||
}
|
||||
void OpenGLGuiHelper::createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld)
|
||||
|
||||
@@ -55,9 +55,27 @@ struct OpenGLGuiHelper : public GUIHelperInterface
|
||||
|
||||
virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
|
||||
|
||||
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual void removeUserDebugItem( int debugItemUniqueId)
|
||||
{
|
||||
}
|
||||
virtual void removeAllUserDebugItems( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld);
|
||||
|
||||
void setVRMode(bool vrMode);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //OPENGL_GUI_HELPER_H
|
||||
|
||||
236
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
Normal file
236
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
Normal file
@@ -0,0 +1,236 @@
|
||||
|
||||
#include "ImportMJCFSetup.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
//#define TEST_MULTIBODY_SERIALIZATION 1
|
||||
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||
#include "../../Utils/b3ResourcePath.h"
|
||||
|
||||
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
||||
|
||||
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
|
||||
|
||||
class ImportMJCFSetup : public CommonMultiBodyBase
|
||||
{
|
||||
char m_fileName[1024];
|
||||
|
||||
struct ImportMJCFInternalData* m_data;
|
||||
bool m_useMultiBody;
|
||||
btAlignedObjectArray<std::string* > m_nameMemory;
|
||||
btScalar m_grav;
|
||||
int m_upAxis;
|
||||
public:
|
||||
ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
|
||||
virtual ~ImportMJCFSetup();
|
||||
|
||||
virtual void initPhysics();
|
||||
virtual void stepSimulation(float deltaTime);
|
||||
|
||||
void setFileName(const char* mjcfFileName);
|
||||
|
||||
virtual void resetCamera()
|
||||
{
|
||||
float dist = 3.5;
|
||||
float pitch = -136;
|
||||
float yaw = 28;
|
||||
float targetPos[3]={0.47,0,-0.64};
|
||||
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
static btAlignedObjectArray<std::string> gMCFJFileNameArray;
|
||||
|
||||
|
||||
#define MAX_NUM_MOTORS 1024
|
||||
|
||||
struct ImportMJCFInternalData
|
||||
{
|
||||
ImportMJCFInternalData()
|
||||
:m_numMotors(0),
|
||||
m_mb(0)
|
||||
{
|
||||
for (int i=0;i<MAX_NUM_MOTORS;i++)
|
||||
{
|
||||
m_jointMotors[i] = 0;
|
||||
m_generic6DofJointMotors[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
|
||||
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
|
||||
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
|
||||
int m_numMotors;
|
||||
btMultiBody* m_mb;
|
||||
btRigidBody* m_rb;
|
||||
|
||||
};
|
||||
|
||||
|
||||
ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
|
||||
:CommonMultiBodyBase(helper),
|
||||
m_grav(0),
|
||||
m_upAxis(2)
|
||||
{
|
||||
m_data = new ImportMJCFInternalData;
|
||||
|
||||
if (option==1)
|
||||
{
|
||||
m_useMultiBody = true;
|
||||
} else
|
||||
{
|
||||
m_useMultiBody = false;
|
||||
}
|
||||
|
||||
static int count = 0;
|
||||
if (fileName)
|
||||
{
|
||||
setFileName(fileName);
|
||||
} else
|
||||
{
|
||||
gMCFJFileNameArray.clear();
|
||||
|
||||
|
||||
|
||||
//load additional MJCF file names from file
|
||||
|
||||
FILE* f = fopen("mjcf_files.txt","r");
|
||||
if (f)
|
||||
{
|
||||
int result;
|
||||
//warning: we don't avoid string buffer overflow in this basic example in fscanf
|
||||
char fileName[1024];
|
||||
do
|
||||
{
|
||||
result = fscanf(f,"%s",fileName);
|
||||
b3Printf("mjcf_files.txt entry %s",fileName);
|
||||
if (result==1)
|
||||
{
|
||||
gMCFJFileNameArray.push_back(fileName);
|
||||
}
|
||||
} while (result==1);
|
||||
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
if (gMCFJFileNameArray.size()==0)
|
||||
{
|
||||
gMCFJFileNameArray.push_back("quadruped/quadruped.mjcf");
|
||||
|
||||
}
|
||||
|
||||
int numFileNames = gMCFJFileNameArray.size();
|
||||
|
||||
if (count>=numFileNames)
|
||||
{
|
||||
count=0;
|
||||
}
|
||||
sprintf(m_fileName,"%s",gMCFJFileNameArray[count++].c_str());
|
||||
}
|
||||
}
|
||||
|
||||
ImportMJCFSetup::~ImportMJCFSetup()
|
||||
{
|
||||
for (int i=0;i<m_nameMemory.size();i++)
|
||||
{
|
||||
delete m_nameMemory[i];
|
||||
}
|
||||
m_nameMemory.clear();
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
static btVector4 colors[4] =
|
||||
{
|
||||
btVector4(1,0,0,1),
|
||||
btVector4(0,1,0,1),
|
||||
btVector4(0,1,1,1),
|
||||
btVector4(1,1,0,1),
|
||||
};
|
||||
|
||||
|
||||
static btVector3 selectColor()
|
||||
{
|
||||
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
return color;
|
||||
}
|
||||
|
||||
void ImportMJCFSetup::setFileName(const char* mjcfFileName)
|
||||
{
|
||||
memcpy(m_fileName,mjcfFileName,strlen(mjcfFileName)+1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void ImportMJCFSetup::initPhysics()
|
||||
{
|
||||
|
||||
|
||||
m_guiHelper->setUpAxis(m_upAxis);
|
||||
|
||||
this->createEmptyDynamicsWorld();
|
||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawContactPoints
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
|
||||
if (m_guiHelper->getParameterInterface())
|
||||
{
|
||||
SliderParams slider("Gravity", &m_grav);
|
||||
slider.m_minVal = -10;
|
||||
slider.m_maxVal = 10;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ImportMJCFSetup::stepSimulation(float deltaTime)
|
||||
{
|
||||
if (m_dynamicsWorld)
|
||||
{
|
||||
btVector3 gravity(0, 0, 0);
|
||||
gravity[m_upAxis] = m_grav;
|
||||
m_dynamicsWorld->setGravity(gravity);
|
||||
|
||||
for (int i=0;i<m_data->m_numMotors;i++)
|
||||
{
|
||||
if (m_data->m_jointMotors[i])
|
||||
{
|
||||
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
|
||||
}
|
||||
if (m_data->m_generic6DofJointMotors[i])
|
||||
{
|
||||
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
|
||||
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
|
||||
//jointInfo->
|
||||
}
|
||||
}
|
||||
|
||||
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
|
||||
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
|
||||
}
|
||||
}
|
||||
|
||||
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options)
|
||||
{
|
||||
|
||||
return new ImportMJCFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
|
||||
}
|
||||
8
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h
Normal file
8
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h
Normal file
@@ -0,0 +1,8 @@
|
||||
#ifndef IMPORT_MJCF_SETUP_H
|
||||
#define IMPORT_MJCF_SETUP_H
|
||||
|
||||
|
||||
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
|
||||
#endif //IMPORT_MJCF_SETUP_H
|
||||
@@ -70,6 +70,11 @@ struct SimpleOpenGL2AppInternalData
|
||||
{
|
||||
GLuint m_fontTextureId;
|
||||
GLuint m_largeFontTextureId;
|
||||
int m_upAxis;
|
||||
SimpleOpenGL2AppInternalData()
|
||||
:m_upAxis(1)
|
||||
{
|
||||
}
|
||||
|
||||
};
|
||||
static GLuint BindFont2(const CTexFont *_Font)
|
||||
@@ -268,10 +273,11 @@ void SimpleOpenGL2App::drawGrid(DrawGridData data)
|
||||
}
|
||||
void SimpleOpenGL2App::setUpAxis(int axis)
|
||||
{
|
||||
this->m_data->m_upAxis = axis;
|
||||
}
|
||||
int SimpleOpenGL2App::getUpAxis() const
|
||||
{
|
||||
return 1;
|
||||
return this->m_data->m_upAxis;
|
||||
}
|
||||
|
||||
void SimpleOpenGL2App::swapBuffer()
|
||||
|
||||
@@ -33,6 +33,7 @@ void SimpleOpenGL2Renderer::updateCamera(int upAxis)
|
||||
float projection[16];
|
||||
float view[16];
|
||||
m_camera.setAspectRatio((float)m_width/(float)m_height);
|
||||
m_camera.setCameraUpAxis(upAxis);
|
||||
m_camera.update();
|
||||
m_camera.getCameraProjectionMatrix(projection);
|
||||
m_camera.getCameraViewMatrix(view);
|
||||
|
||||
@@ -200,7 +200,6 @@ void RaytestDemo::initPhysics()
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
body->setRollingFriction(1);
|
||||
body->setFriction(1);
|
||||
//add the body to the dynamics world
|
||||
m_dynamicsWorld->addRigidBody(body);
|
||||
@@ -269,6 +268,7 @@ void RaytestDemo::initPhysics()
|
||||
rbInfo.m_startWorldTransform = startTransform;
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
body->setRollingFriction(0.03);
|
||||
body->setSpinningFriction(0.03);
|
||||
body->setFriction(1);
|
||||
body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(),btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
|
||||
|
||||
@@ -401,6 +401,21 @@ public:
|
||||
|
||||
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
|
||||
{
|
||||
}
|
||||
|
||||
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual void removeUserDebugItem( int debugItemUniqueId)
|
||||
{
|
||||
}
|
||||
virtual void removeAllUserDebugItems( )
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -79,6 +79,87 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
|
||||
return 0;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
|
||||
if (cl->canSubmitCommand())
|
||||
{
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_LOAD_BULLET;
|
||||
int len = strlen(fileName);
|
||||
if (len < MAX_URDF_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_fileArguments.m_fileName, fileName);
|
||||
}
|
||||
else
|
||||
{
|
||||
command->m_fileArguments.m_fileName[0] = 0;
|
||||
}
|
||||
command->m_updateFlags = 0;
|
||||
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
|
||||
if (cl->canSubmitCommand())
|
||||
{
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_SAVE_BULLET;
|
||||
int len = strlen(fileName);
|
||||
if (len < MAX_URDF_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_fileArguments.m_fileName, fileName);
|
||||
}
|
||||
else
|
||||
{
|
||||
command->m_fileArguments.m_fileName[0] = 0;
|
||||
}
|
||||
command->m_updateFlags = 0;
|
||||
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
|
||||
if (cl->canSubmitCommand())
|
||||
{
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_LOAD_MJCF;
|
||||
int len = strlen(fileName);
|
||||
if (len < MAX_URDF_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_fileArguments.m_fileName, fileName);
|
||||
}
|
||||
else
|
||||
{
|
||||
command->m_fileArguments.m_fileName[0] = 0;
|
||||
}
|
||||
command->m_updateFlags = 0;
|
||||
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
@@ -712,6 +793,7 @@ int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyInd
|
||||
{
|
||||
switch (status->m_type)
|
||||
{
|
||||
case CMD_BULLET_LOADING_COMPLETED:
|
||||
case CMD_SDF_LOADING_COMPLETED:
|
||||
{
|
||||
int i,maxBodies;
|
||||
@@ -981,6 +1063,107 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
|
||||
|
||||
}
|
||||
|
||||
|
||||
/// Add/remove user-specific debug lines and debug text messages
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type =CMD_USER_DEBUG_DRAW;
|
||||
command->m_updateFlags = USER_DEBUG_HAS_LINE; //USER_DEBUG_HAS_TEXT
|
||||
|
||||
command->m_userDebugDrawArgs.m_debugLineFromXYZ[0] = fromXYZ[0];
|
||||
command->m_userDebugDrawArgs.m_debugLineFromXYZ[1] = fromXYZ[1];
|
||||
command->m_userDebugDrawArgs.m_debugLineFromXYZ[2] = fromXYZ[2];
|
||||
|
||||
command->m_userDebugDrawArgs.m_debugLineToXYZ[0] = toXYZ[0];
|
||||
command->m_userDebugDrawArgs.m_debugLineToXYZ[1] = toXYZ[1];
|
||||
command->m_userDebugDrawArgs.m_debugLineToXYZ[2] = toXYZ[2];
|
||||
|
||||
command->m_userDebugDrawArgs.m_debugLineColorRGB[0] = colorRGB[0];
|
||||
command->m_userDebugDrawArgs.m_debugLineColorRGB[1] = colorRGB[1];
|
||||
command->m_userDebugDrawArgs.m_debugLineColorRGB[2] = colorRGB[2];
|
||||
|
||||
command->m_userDebugDrawArgs.m_lineWidth = lineWidth;
|
||||
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
|
||||
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime)
|
||||
{
|
||||
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type =CMD_USER_DEBUG_DRAW;
|
||||
command->m_updateFlags = USER_DEBUG_HAS_TEXT;
|
||||
|
||||
int len = strlen(txt);
|
||||
if (len<MAX_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_userDebugDrawArgs.m_text,txt);
|
||||
} else
|
||||
{
|
||||
command->m_userDebugDrawArgs.m_text[0] = 0;
|
||||
}
|
||||
command->m_userDebugDrawArgs.m_textPositionXYZ[0] = positionXYZ[0];
|
||||
command->m_userDebugDrawArgs.m_textPositionXYZ[1] = positionXYZ[1];
|
||||
command->m_userDebugDrawArgs.m_textPositionXYZ[2] = positionXYZ[2];
|
||||
|
||||
command->m_userDebugDrawArgs.m_textColorRGB[0] = colorRGB[0];
|
||||
command->m_userDebugDrawArgs.m_textColorRGB[1] = colorRGB[1];
|
||||
command->m_userDebugDrawArgs.m_textColorRGB[2] = colorRGB[2];
|
||||
|
||||
command->m_userDebugDrawArgs.m_textSize = textSize;
|
||||
|
||||
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
|
||||
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type =CMD_USER_DEBUG_DRAW;
|
||||
command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
|
||||
command->m_userDebugDrawArgs.m_removeItemUniqueId = debugItemUniqueId;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type =CMD_USER_DEBUG_DRAW;
|
||||
command->m_updateFlags = USER_DEBUG_REMOVE_ALL;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
|
||||
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED);
|
||||
if (status->m_type != CMD_USER_DEBUG_DRAW_COMPLETED)
|
||||
return -1;
|
||||
|
||||
return status->m_userDebugDrawArgs.m_debugItemUniqueId;
|
||||
}
|
||||
|
||||
|
||||
///request an image from a simulated camera, using a software renderer.
|
||||
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
@@ -1546,6 +1729,7 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
|
||||
|
||||
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
|
||||
|
||||
@@ -74,14 +74,22 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
|
||||
|
||||
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
|
||||
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
|
||||
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
|
||||
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
|
||||
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
|
||||
|
||||
///Get the pointers to the debug line information, after b3InitRequestDebugLinesCommand returns
|
||||
///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns
|
||||
///status CMD_DEBUG_LINES_COMPLETED
|
||||
void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
|
||||
|
||||
/// Add/remove user-specific debug lines and debug text messages
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime);
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
|
||||
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
|
||||
///All debug items unique Ids are positive: a negative unique Id means failure.
|
||||
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
|
||||
///request an image from a simulated camera, using a software renderer.
|
||||
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
|
||||
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
|
||||
@@ -143,6 +151,11 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
|
||||
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
||||
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
|
||||
|
||||
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
||||
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
||||
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
||||
|
||||
|
||||
|
||||
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
|
||||
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
|
||||
@@ -243,6 +256,7 @@ b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandl
|
||||
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
|
||||
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
|
||||
|
||||
///experiments of robots interacting with non-rigid objects (such as btSoftBody)
|
||||
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
|
||||
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
|
||||
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
|
||||
|
||||
@@ -703,6 +703,35 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("Load texture failed");
|
||||
break;
|
||||
}
|
||||
case CMD_BULLET_LOADING_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_BULLET_LOADING_FAILED:
|
||||
{
|
||||
b3Warning("Load .bullet failed");
|
||||
break;
|
||||
}
|
||||
case CMD_BULLET_SAVING_FAILED:
|
||||
{
|
||||
b3Warning("Save .bullet failed");
|
||||
break;
|
||||
}
|
||||
case CMD_MJCF_LOADING_FAILED:
|
||||
{
|
||||
b3Warning("Load .mjcf failed");
|
||||
break;
|
||||
}
|
||||
case CMD_USER_DEBUG_DRAW_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_USER_DEBUG_DRAW_FAILED:
|
||||
{
|
||||
b3Warning("User debug draw failed");
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
|
||||
@@ -662,10 +662,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CMD_BULLET_LOADING_FAILED:
|
||||
{
|
||||
b3Warning("Couldn't load .bullet file");
|
||||
break;
|
||||
}
|
||||
case CMD_BULLET_LOADING_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
// b3Error("Unknown server status type");
|
||||
b3Warning("Unknown server status type");
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -3363,14 +3363,174 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
|
||||
|
||||
m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName);
|
||||
int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName);
|
||||
|
||||
serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED;
|
||||
if (uid>=0)
|
||||
{
|
||||
serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED;
|
||||
} else
|
||||
{
|
||||
serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
|
||||
}
|
||||
hasStatus = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_LOAD_BULLET:
|
||||
{
|
||||
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
|
||||
|
||||
const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
|
||||
int numPrefixes = sizeof(prefix) / sizeof(const char*);
|
||||
char relativeFileName[1024];
|
||||
FILE* f = 0;
|
||||
bool found = false;
|
||||
|
||||
for (int i = 0; !f && i<numPrefixes; i++)
|
||||
{
|
||||
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
|
||||
f = fopen(relativeFileName, "rb");
|
||||
if (f)
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
if (found)
|
||||
{
|
||||
bool ok = importer->loadFile(relativeFileName);
|
||||
if (ok)
|
||||
{
|
||||
|
||||
|
||||
int numRb = importer->getNumRigidBodies();
|
||||
serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
|
||||
|
||||
for( int i=0;i<numRb;i++)
|
||||
{
|
||||
btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
|
||||
|
||||
if (colObj)
|
||||
{
|
||||
btRigidBody* rb = btRigidBody::upcast(colObj);
|
||||
if (rb)
|
||||
{
|
||||
int bodyUniqueId = m_data->allocHandle();
|
||||
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
||||
colObj->setUserIndex2(bodyUniqueId);
|
||||
bodyHandle->m_rigidBody = rb;
|
||||
|
||||
if (serverStatusOut.m_sdfLoadedArgs.m_numBodies<MAX_SDF_BODIES)
|
||||
{
|
||||
serverStatusOut.m_sdfLoadedArgs.m_numBodies++;
|
||||
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = bodyUniqueId;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
serverCmd.m_type = CMD_BULLET_LOADING_COMPLETED;
|
||||
m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
serverCmd.m_type = CMD_BULLET_LOADING_FAILED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_SAVE_BULLET:
|
||||
{
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
|
||||
FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb");
|
||||
if (f)
|
||||
{
|
||||
btDefaultSerializer* ser = new btDefaultSerializer();
|
||||
m_data->m_dynamicsWorld->serialize(ser);
|
||||
fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
|
||||
fclose(f);
|
||||
serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED;
|
||||
delete ser;
|
||||
}
|
||||
serverCmd.m_type = CMD_BULLET_SAVING_FAILED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_LOAD_MJCF:
|
||||
{
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_MJCF_LOADING_FAILED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_USER_DEBUG_DRAW:
|
||||
{
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
|
||||
hasStatus = true;
|
||||
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT)
|
||||
{
|
||||
int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text,
|
||||
clientCmd.m_userDebugDrawArgs.m_textPositionXYZ,
|
||||
clientCmd.m_userDebugDrawArgs.m_textColorRGB,
|
||||
clientCmd.m_userDebugDrawArgs.m_textSize,
|
||||
clientCmd.m_userDebugDrawArgs.m_lifeTime);
|
||||
|
||||
if (uid>=0)
|
||||
{
|
||||
serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE)
|
||||
{
|
||||
int uid = m_data->m_guiHelper->addUserDebugLine(
|
||||
clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ,
|
||||
clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ,
|
||||
clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB,
|
||||
clientCmd.m_userDebugDrawArgs.m_lineWidth,
|
||||
clientCmd.m_userDebugDrawArgs.m_lifeTime);
|
||||
|
||||
if (uid>=0)
|
||||
{
|
||||
serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
|
||||
{
|
||||
m_data->m_guiHelper->removeAllUserDebugItems();
|
||||
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
|
||||
|
||||
}
|
||||
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
|
||||
{
|
||||
m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_removeItemUniqueId);
|
||||
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
default:
|
||||
{
|
||||
b3Error("Unknown command encountered");
|
||||
|
||||
@@ -74,6 +74,11 @@ enum MultiThreadedGUIHelperCommunicationEnums
|
||||
eGUIHelperCreateRigidBodyGraphicsObject,
|
||||
eGUIHelperRemoveAllGraphicsInstances,
|
||||
eGUIHelperCopyCameraImageData,
|
||||
eGUIHelperAutogenerateGraphicsObjects,
|
||||
eGUIUserDebugAddText,
|
||||
eGUIUserDebugAddLine,
|
||||
eGUIUserDebugRemoveItem,
|
||||
eGUIUserDebugRemoveAllItems,
|
||||
};
|
||||
|
||||
#include <stdio.h>
|
||||
@@ -270,6 +275,30 @@ void* MotionlsMemoryFunc()
|
||||
|
||||
|
||||
|
||||
struct UserDebugDrawLine
|
||||
{
|
||||
double m_debugLineFromXYZ[3];
|
||||
double m_debugLineToXYZ[3];
|
||||
double m_debugLineColorRGB[3];
|
||||
double m_lineWidth;
|
||||
|
||||
double m_lifeTime;
|
||||
int m_itemUniqueId;
|
||||
};
|
||||
|
||||
struct UserDebugText
|
||||
{
|
||||
char m_text[1024];
|
||||
double m_textPositionXYZ[3];
|
||||
double m_textColorRGB[3];
|
||||
double textSize;
|
||||
|
||||
double m_lifeTime;
|
||||
int m_itemUniqueId;
|
||||
};
|
||||
|
||||
|
||||
|
||||
class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
|
||||
{
|
||||
CommonGraphicsApp* m_app;
|
||||
@@ -277,10 +306,12 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
|
||||
b3CriticalSection* m_cs;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
GUIHelperInterface* m_childGuiHelper;
|
||||
|
||||
int m_uidGenerator;
|
||||
const unsigned char* m_texels;
|
||||
int m_textureWidth;
|
||||
int m_textureHeight;
|
||||
@@ -304,6 +335,7 @@ public:
|
||||
MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
|
||||
:m_app(app)
|
||||
,m_cs(0),
|
||||
m_uidGenerator(0),
|
||||
m_texels(0),
|
||||
m_textureId(-1)
|
||||
{
|
||||
@@ -530,13 +562,113 @@ public:
|
||||
}
|
||||
|
||||
|
||||
btDiscreteDynamicsWorld* m_dynamicsWorld;
|
||||
|
||||
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
|
||||
{
|
||||
m_dynamicsWorld = rbWorld;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btAlignedObjectArray<UserDebugText> m_userDebugText;
|
||||
|
||||
UserDebugText m_tmpText;
|
||||
|
||||
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
|
||||
{
|
||||
|
||||
m_tmpText.m_itemUniqueId = m_uidGenerator++;
|
||||
m_tmpText.m_lifeTime = lifeTime;
|
||||
m_tmpText.textSize = size;
|
||||
int len = strlen(txt);
|
||||
strcpy(m_tmpText.m_text,txt);
|
||||
m_tmpText.m_textPositionXYZ[0] = positionXYZ[0];
|
||||
m_tmpText.m_textPositionXYZ[1] = positionXYZ[1];
|
||||
m_tmpText.m_textPositionXYZ[2] = positionXYZ[2];
|
||||
m_tmpText.m_textColorRGB[0] = textColorRGB[0];
|
||||
m_tmpText.m_textColorRGB[1] = textColorRGB[1];
|
||||
m_tmpText.m_textColorRGB[2] = textColorRGB[2];
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugAddText);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
|
||||
return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
|
||||
}
|
||||
|
||||
btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
|
||||
UserDebugDrawLine m_tmpLine;
|
||||
|
||||
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )
|
||||
{
|
||||
m_tmpLine.m_lifeTime = lifeTime;
|
||||
m_tmpLine.m_lineWidth = lineWidth;
|
||||
m_tmpLine.m_itemUniqueId = m_uidGenerator++;
|
||||
m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
|
||||
m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
|
||||
m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
|
||||
|
||||
m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
|
||||
m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
|
||||
m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
|
||||
|
||||
m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
|
||||
m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
|
||||
m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
|
||||
}
|
||||
|
||||
int m_removeDebugItemUid;
|
||||
|
||||
virtual void removeUserDebugItem( int debugItemUniqueId)
|
||||
{
|
||||
m_removeDebugItemUid = debugItemUniqueId;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
|
||||
}
|
||||
virtual void removeAllUserDebugItems( )
|
||||
{
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -916,10 +1048,76 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperAutogenerateGraphicsObjects:
|
||||
{
|
||||
m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld);
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
|
||||
case eGUIUserDebugAddText:
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText);
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
case eGUIUserDebugAddLine:
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
case eGUIUserDebugRemoveItem:
|
||||
{
|
||||
for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
|
||||
{
|
||||
if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
|
||||
m_multiThreadedHelper->m_userDebugLines.pop_back();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
|
||||
{
|
||||
if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
|
||||
m_multiThreadedHelper->m_userDebugText.pop_back();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
case eGUIUserDebugRemoveAllItems:
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugLines.clear();
|
||||
m_multiThreadedHelper->m_userDebugText.clear();
|
||||
m_multiThreadedHelper->m_uidGenerator = 0;
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperIdle:
|
||||
{
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -974,15 +1172,38 @@ void PhysicsServerExample::renderScene()
|
||||
//add array of lines
|
||||
|
||||
//draw all user- 'text3d' messages
|
||||
if (m_guiHelper)
|
||||
if (m_multiThreadedHelper)
|
||||
{
|
||||
btVector3 from(0, 0, 0);
|
||||
btVector3 toX(1, 1, 1);
|
||||
btVector3 color(0, 1, 0);
|
||||
double width = 2;
|
||||
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, width);
|
||||
|
||||
m_guiHelper->getAppInterface()->drawText3D("hi", 1, 1, 1, 1);
|
||||
for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
|
||||
{
|
||||
btVector3 from;
|
||||
from.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
|
||||
btVector3 toX;
|
||||
toX.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
|
||||
|
||||
btVector3 color;
|
||||
color.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
|
||||
|
||||
|
||||
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
|
||||
}
|
||||
|
||||
for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
|
||||
{
|
||||
m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
|
||||
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
|
||||
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
|
||||
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
|
||||
m_multiThreadedHelper->m_userDebugText[i].textSize);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -67,6 +67,11 @@ struct SdfArgs
|
||||
int m_useMultiBody;
|
||||
};
|
||||
|
||||
struct FileArgs
|
||||
{
|
||||
char m_fileName[MAX_URDF_FILENAME_LENGTH];
|
||||
};
|
||||
|
||||
enum EnumUrdfArgsUpdateFlags
|
||||
{
|
||||
URDF_ARGS_FILE_NAME=1,
|
||||
@@ -495,6 +500,40 @@ struct CreateJointArgs
|
||||
int m_jointType;
|
||||
};
|
||||
|
||||
|
||||
|
||||
enum EnumUserDebugDrawFlags
|
||||
{
|
||||
USER_DEBUG_HAS_LINE=1,
|
||||
USER_DEBUG_HAS_TEXT=2,
|
||||
USER_DEBUG_REMOVE_ONE_ITEM=4,
|
||||
USER_DEBUG_REMOVE_ALL=8
|
||||
};
|
||||
|
||||
struct UserDebugDrawArgs
|
||||
{
|
||||
double m_debugLineFromXYZ[3];
|
||||
double m_debugLineToXYZ[3];
|
||||
double m_debugLineColorRGB[3];
|
||||
double m_lineWidth;
|
||||
|
||||
double m_lifeTime;
|
||||
int m_removeItemUniqueId;
|
||||
|
||||
char m_text[MAX_FILENAME_LENGTH];
|
||||
double m_textPositionXYZ[3];
|
||||
double m_textColorRGB[3];
|
||||
double m_textSize;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct UserDebugDrawResultArgs
|
||||
{
|
||||
int m_debugItemUniqueId;
|
||||
};
|
||||
|
||||
|
||||
struct SharedMemoryCommand
|
||||
{
|
||||
int m_type;
|
||||
@@ -509,6 +548,7 @@ struct SharedMemoryCommand
|
||||
{
|
||||
struct UrdfArgs m_urdfArguments;
|
||||
struct SdfArgs m_sdfArguments;
|
||||
struct FileArgs m_fileArguments;
|
||||
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
|
||||
struct InitPoseArgs m_initPoseArgs;
|
||||
struct SendPhysicsSimulationParameters m_physSimParamArgs;
|
||||
@@ -530,6 +570,7 @@ struct SharedMemoryCommand
|
||||
struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
|
||||
struct LoadTextureArgs m_loadTextureArguments;
|
||||
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
|
||||
struct UserDebugDrawArgs m_userDebugDrawArgs;
|
||||
struct LoadBunnyArgs m_loadBunnyArguments;
|
||||
};
|
||||
};
|
||||
@@ -578,6 +619,7 @@ struct SharedMemoryStatus
|
||||
struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
|
||||
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
|
||||
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
|
||||
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@@ -7,19 +7,20 @@ enum EnumSharedMemoryClientCommand
|
||||
{
|
||||
CMD_LOAD_SDF,
|
||||
CMD_LOAD_URDF,
|
||||
CMD_LOAD_BULLET,
|
||||
CMD_SAVE_BULLET,
|
||||
CMD_LOAD_MJCF,
|
||||
CMD_LOAD_BUNNY,
|
||||
CMD_SEND_BULLET_DATA_STREAM,
|
||||
CMD_CREATE_BOX_COLLISION_SHAPE,
|
||||
// CMD_DELETE_BOX_COLLISION_SHAPE,
|
||||
CMD_CREATE_RIGID_BODY,
|
||||
CMD_DELETE_RIGID_BODY,
|
||||
CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
|
||||
// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED
|
||||
CMD_INIT_POSE,
|
||||
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
|
||||
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
|
||||
CMD_REQUEST_ACTUAL_STATE,
|
||||
CMD_REQUEST_DEBUG_LINES,
|
||||
CMD_SEND_BULLET_DATA_STREAM,
|
||||
CMD_CREATE_BOX_COLLISION_SHAPE,
|
||||
CMD_CREATE_RIGID_BODY,
|
||||
CMD_DELETE_RIGID_BODY,
|
||||
CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
|
||||
CMD_INIT_POSE,
|
||||
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
|
||||
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
|
||||
CMD_REQUEST_ACTUAL_STATE,
|
||||
CMD_REQUEST_DEBUG_LINES,
|
||||
CMD_REQUEST_BODY_INFO,
|
||||
CMD_REQUEST_INTERNAL_DATA,
|
||||
CMD_STEP_FORWARD_SIMULATION,
|
||||
@@ -39,6 +40,8 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_REQUEST_VISUAL_SHAPE_INFO,
|
||||
CMD_UPDATE_VISUAL_SHAPE,
|
||||
CMD_LOAD_TEXTURE,
|
||||
CMD_USER_DEBUG_DRAW,
|
||||
|
||||
//don't go beyond this command!
|
||||
CMD_MAX_CLIENT_COMMANDS,
|
||||
|
||||
@@ -56,6 +59,12 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_SDF_LOADING_FAILED,
|
||||
CMD_URDF_LOADING_COMPLETED,
|
||||
CMD_URDF_LOADING_FAILED,
|
||||
CMD_BULLET_LOADING_COMPLETED,
|
||||
CMD_BULLET_LOADING_FAILED,
|
||||
CMD_BULLET_SAVING_COMPLETED,
|
||||
CMD_BULLET_SAVING_FAILED,
|
||||
CMD_MJCF_LOADING_COMPLETED,
|
||||
CMD_MJCF_LOADING_FAILED,
|
||||
CMD_REQUEST_INTERNAL_DATA_COMPLETED,
|
||||
CMD_REQUEST_INTERNAL_DATA_FAILED,
|
||||
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
|
||||
@@ -93,6 +102,8 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_VISUAL_SHAPE_UPDATE_FAILED,
|
||||
CMD_LOAD_TEXTURE_COMPLETED,
|
||||
CMD_LOAD_TEXTURE_FAILED,
|
||||
CMD_USER_DEBUG_DRAW_COMPLETED,
|
||||
CMD_USER_DEBUG_DRAW_FAILED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
@@ -232,6 +243,7 @@ struct b3VisualShapeData
|
||||
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
|
||||
double m_localInertiaFrame[7];//pos[3], orn[4]
|
||||
//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
|
||||
double m_rgbaColor[4];
|
||||
};
|
||||
|
||||
struct b3VisualShapeInformation
|
||||
|
||||
@@ -527,6 +527,10 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
||||
visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1];
|
||||
visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2];
|
||||
visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3];
|
||||
visualShape.m_rgbaColor[0] = rgbaColor[0];
|
||||
visualShape.m_rgbaColor[1] = rgbaColor[1];
|
||||
visualShape.m_rgbaColor[2] = rgbaColor[2];
|
||||
visualShape.m_rgbaColor[3] = rgbaColor[3];
|
||||
|
||||
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
|
||||
m_data->m_visualShapes.push_back(visualShape);
|
||||
@@ -872,10 +876,14 @@ int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int
|
||||
return m_data->m_textures.size()-1;
|
||||
}
|
||||
|
||||
void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
|
||||
int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
|
||||
{
|
||||
int width,height,n;
|
||||
unsigned char* image=0;
|
||||
image = stbi_load(filename, &width, &height, &n, 3);
|
||||
registerTexture(image, width, height);
|
||||
if (image && (width>=0) && (height>=0))
|
||||
{
|
||||
return registerTexture(image, width, height);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
@@ -38,7 +38,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
|
||||
void render();
|
||||
void render(const float viewMat[16], const float projMat[16]);
|
||||
|
||||
void loadTextureFile(const char* filename);
|
||||
int loadTextureFile(const char* filename);
|
||||
int registerTexture(unsigned char* texels, int width, int height);
|
||||
void activateShapeTexture(int shapeUniqueId, int textureUniqueId);
|
||||
void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
|
||||
|
||||
@@ -94,7 +94,8 @@ int main(int argc, char *argv[])
|
||||
|
||||
switch (event.type)
|
||||
{
|
||||
case ENET_EVENT_TYPE_CONNECT:
|
||||
case ENET_EVENT_TYPE_CONNECT:
|
||||
{
|
||||
printf("A new client connected from %x:%u.\n",
|
||||
event.peer->address.host,
|
||||
event.peer->address.port);
|
||||
@@ -103,8 +104,9 @@ int main(int argc, char *argv[])
|
||||
event.peer->data = (void*)"Client information";
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
case ENET_EVENT_TYPE_RECEIVE:
|
||||
{
|
||||
if (gVerboseNetworkMessagesServer)
|
||||
{
|
||||
printf("A packet of length %u containing '%s' was "
|
||||
@@ -171,8 +173,9 @@ int main(int argc, char *argv[])
|
||||
//enet_host_broadcast(server, 0, event.packet);
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
case ENET_EVENT_TYPE_DISCONNECT:
|
||||
{
|
||||
printf("%s disconnected.\n", event.peer->data);
|
||||
|
||||
/* Reset the peer's client information. */
|
||||
@@ -180,6 +183,11 @@ int main(int argc, char *argv[])
|
||||
event.peer->data = NULL;
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (serviceResult > 0)
|
||||
|
||||
@@ -2131,12 +2131,12 @@ void SoftDemo::initPhysics()
|
||||
for (int j=0;j<NUM_VERTS_Y-1;j++)
|
||||
{
|
||||
gGroundIndices[index++] = j*NUM_VERTS_X+i;
|
||||
gGroundIndices[index++] = j*NUM_VERTS_X+i+1;
|
||||
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
|
||||
gGroundIndices[index++] = j*NUM_VERTS_X+i+1;;
|
||||
|
||||
gGroundIndices[index++] = j*NUM_VERTS_X+i;
|
||||
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
|
||||
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i;
|
||||
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -379,6 +379,20 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
|
||||
|
||||
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
|
||||
{
|
||||
}
|
||||
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual void removeUserDebugItem( int debugItemUniqueId)
|
||||
{
|
||||
}
|
||||
virtual void removeAllUserDebugItems( )
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -197,12 +197,112 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
|
||||
PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
|
||||
return NULL;
|
||||
|
||||
}
|
||||
|
||||
#define MAX_SDF_BODIES 512
|
||||
static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
const char* bulletFileName = "";
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command;
|
||||
int i,numBodies;
|
||||
int bodyIndicesOut[MAX_SDF_BODIES];
|
||||
PyObject* pylist = 0;
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
if (size == 1) {
|
||||
if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
|
||||
}
|
||||
|
||||
command = b3LoadBulletCommandInit(sm, bulletFileName);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType != CMD_BULLET_LOADING_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Couldn't load .bullet file.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
numBodies =
|
||||
b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
|
||||
if (numBodies > MAX_SDF_BODIES) {
|
||||
PyErr_SetString(SpamError, "loadBullet exceeds body capacity");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pylist = PyTuple_New(numBodies);
|
||||
|
||||
if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) {
|
||||
for (i = 0; i < numBodies; i++) {
|
||||
PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
|
||||
}
|
||||
}
|
||||
return pylist;
|
||||
|
||||
|
||||
}
|
||||
|
||||
static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
const char* bulletFileName = "";
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command;
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
if (size == 1) {
|
||||
if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
|
||||
}
|
||||
command = b3SaveBulletCommandInit(sm, bulletFileName);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
if (statusHandle != CMD_BULLET_SAVING_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Couldn't save .bullet file.");
|
||||
return NULL;
|
||||
}
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Load a URDF file indicating the links and joints of an object
|
||||
static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
const char* mjcfjFileName = "";
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command;
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
if (size == 1) {
|
||||
if (!PyArg_ParseTuple(args, "s", &mjcfjFileName)) return NULL;
|
||||
}
|
||||
command = b3LoadMJCFCommandInit(sm, mjcfjFileName);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
if (statusHandle != CMD_MJCF_LOADING_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
|
||||
return NULL;
|
||||
}
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Load a robot from a URDF file (universal robot description format)
|
||||
// function can be called without arguments and will default
|
||||
// to position (0,0,1) with orientation(0,0,0,1)
|
||||
// els(x,y,z) or
|
||||
@@ -283,7 +383,7 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
|
||||
return v;
|
||||
}
|
||||
|
||||
#define MAX_SDF_BODIES 512
|
||||
|
||||
|
||||
static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) {
|
||||
const char* sdfFileName = "";
|
||||
@@ -1293,12 +1393,14 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
|
||||
//
|
||||
// // Args:
|
||||
// vector - float[3] which will be set by values from objMat
|
||||
static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
|
||||
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
|
||||
int i, len;
|
||||
PyObject* seq;
|
||||
if (objVec==NULL)
|
||||
return 0;
|
||||
|
||||
seq = PySequence_Fast(objMat, "expected a sequence");
|
||||
len = PySequence_Size(objMat);
|
||||
seq = PySequence_Fast(objVec, "expected a sequence");
|
||||
len = PySequence_Size(objVec);
|
||||
if (len == 3) {
|
||||
for (i = 0; i < len; i++) {
|
||||
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
||||
@@ -1314,6 +1416,8 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
|
||||
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
|
||||
int i, len;
|
||||
PyObject* seq;
|
||||
if (obVec==NULL)
|
||||
return 0;
|
||||
|
||||
seq = PySequence_Fast(obVec, "expected a sequence");
|
||||
len = PySequence_Size(obVec);
|
||||
@@ -1332,6 +1436,8 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
|
||||
static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
|
||||
int i, len;
|
||||
PyObject* seq;
|
||||
if (obVec==NULL)
|
||||
return 0;
|
||||
|
||||
seq = PySequence_Fast(obVec, "expected a sequence");
|
||||
len = PySequence_Size(obVec);
|
||||
@@ -1346,6 +1452,196 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int res = 0;
|
||||
|
||||
PyObject* pyResultList = 0;
|
||||
char* text;
|
||||
double posXYZ[3];
|
||||
double colorRGB[3]={1,1,1};
|
||||
|
||||
|
||||
PyObject* textPositionObj=0;
|
||||
PyObject* textColorRGBObj=0;
|
||||
double textSize = 1.f;
|
||||
double lifeTime = 0.f;
|
||||
|
||||
static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL };
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
res = pybullet_internalSetVectord(textPositionObj,posXYZ);
|
||||
if (!res)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (textColorRGBObj)
|
||||
{
|
||||
res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
|
||||
if (!res)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error converting lineTo[3]");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
|
||||
{
|
||||
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
||||
PyObject* item = PyInt_FromLong(debugItemUniqueId);
|
||||
return item;
|
||||
}
|
||||
|
||||
PyErr_SetString(SpamError, "Error in addUserDebugText.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int res = 0;
|
||||
|
||||
PyObject* pyResultList = 0;
|
||||
|
||||
double fromXYZ[3];
|
||||
double toXYZ[3];
|
||||
double colorRGB[3]={1,1,1};
|
||||
|
||||
PyObject* lineFromObj=0;
|
||||
PyObject* lineToObj=0;
|
||||
PyObject* lineColorRGBObj=0;
|
||||
double lineWidth = 1.f;
|
||||
double lifeTime = 0.f;
|
||||
|
||||
static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL };
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
res = pybullet_internalSetVectord(lineFromObj,fromXYZ);
|
||||
if (!res)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
res = pybullet_internalSetVectord(lineToObj,toXYZ);
|
||||
if (!res)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error converting lineTo[3]");
|
||||
return NULL;
|
||||
}
|
||||
if (lineColorRGBObj)
|
||||
{
|
||||
res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB);
|
||||
}
|
||||
|
||||
commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
|
||||
{
|
||||
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
||||
PyObject* item = PyInt_FromLong(debugItemUniqueId);
|
||||
return item;
|
||||
}
|
||||
|
||||
PyErr_SetString(SpamError, "Error in addUserDebugLine.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int itemUniqueId;
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) {
|
||||
PyErr_SetString(SpamError, "Error parsing user debug item unique id");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
commandHandle = b3InitUserDebugDrawRemoveAll(sm);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
@@ -1426,7 +1722,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
|
||||
PyTuple_SetItem(visualShapeObList, 6, vec);
|
||||
}
|
||||
|
||||
|
||||
visualShapeInfo.m_visualShapeData[0].m_rgbaColor[0];
|
||||
|
||||
PyTuple_SetItem(pyResultList, i, visualShapeObList);
|
||||
}
|
||||
@@ -2515,8 +2811,17 @@ static PyMethodDef SpamMethods[] = {
|
||||
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
|
||||
"Load multibodies from an SDF file."},
|
||||
|
||||
{ "loadBullet", pybullet_loadBullet, METH_VARARGS,
|
||||
"Restore the full state of the world from a .bullet file." },
|
||||
|
||||
{ "saveBullet", pybullet_saveBullet, METH_VARARGS,
|
||||
"Save the full state of the world to a .bullet file." },
|
||||
|
||||
{ "loadMJCF", pybullet_loadMJCF, METH_VARARGS,
|
||||
"Load multibodies from an MJCF file." },
|
||||
|
||||
{"saveWorld", pybullet_saveWorld, METH_VARARGS,
|
||||
"Save an approximate Python file to reproduce the current state of the world: saveWorld"
|
||||
"Save a approximate Python file to reproduce the current state of the world: saveWorld"
|
||||
"(filename). (very preliminary and approximately)"},
|
||||
|
||||
{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
|
||||
@@ -2595,6 +2900,30 @@ static PyMethodDef SpamMethods[] = {
|
||||
"axis-aligned bounding box volume (AABB)."
|
||||
"Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."
|
||||
},
|
||||
|
||||
|
||||
{ "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS,
|
||||
"Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
|
||||
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
|
||||
},
|
||||
|
||||
|
||||
{ "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS,
|
||||
"Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds "
|
||||
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
|
||||
},
|
||||
|
||||
{ "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS,
|
||||
"remove a user debug draw item, giving its unique id"
|
||||
},
|
||||
|
||||
|
||||
{ "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS,
|
||||
"remove all user debug draw items"
|
||||
},
|
||||
|
||||
|
||||
|
||||
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
|
||||
"Return the visual shape information for one object." },
|
||||
|
||||
|
||||
Reference in New Issue
Block a user