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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Benjamin Ellenberger
2016-06-27 15:14:47 +02:00
parent e16082a7cd ed9be25570
commit 50b6003470
65 changed files with 2993 additions and 616 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/BasicDemo/BasicExample.cpp
View File

@@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
float dist = 41;
float pitch = 52;
float yaw = 35;
float targetPos[3]={0,0.46,0};
float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
};

1
examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
View File

@@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
}
}
}
delete hull;
}
} else
{

5
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -245,7 +245,10 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
#ifdef ENABLE_LUA

3
examples/ExampleBrowser/main.cpp
View File

@@ -13,6 +13,8 @@
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
int main(int argc, char* argv[])
@@ -29,6 +31,7 @@ int main(int argc, char* argv[])
exampleBrowser->registerFileImporter(".urdf",ImportURDFCreateFunc);
exampleBrowser->registerFileImporter(".sdf",ImportSDFCreateFunc);
exampleBrowser->registerFileImporter(".obj",ImportObjCreateFunc);
exampleBrowser->registerFileImporter(".stl",ImportSTLCreateFunc);
clock.reset();
if (init)

12
examples/ExtendedTutorials/premake4.lua
View File

@@ -38,7 +38,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
@@ -89,7 +90,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
if os.is("Linux") then initX11() end
@@ -153,7 +155,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
if os.is("Linux") then initX11() end
@@ -213,6 +216,7 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}

33
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -1,15 +1,12 @@
#include "b3ImportMeshUtility.h"
#include <vector>
#include "../../OpenGLWindow/GLInstancingRenderer.h"
#include"Wavefront/tiny_obj_loader.h"
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "btBulletDynamicsCommon.h"
#include "../../OpenGLWindow/SimpleOpenGL3App.h"
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "LinearMath/btVector3.h"
#include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h"
#include "../../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3FileUtils.h"
#include "stb_image/stb_image.h"
#include "../../ThirdPartyLibs/stb_image/stb_image.h"
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
@@ -88,28 +85,4 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
return false;
}
int b3ImportMeshUtility::loadAndRegisterMeshFromFile(const std::string& fileName, CommonRenderInterface* renderer)
{
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
{
int textureIndex = -1;
if (meshData.m_textureImage)
{
textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
}
shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,
&meshData.m_gfxShape->m_indices->at(0),
meshData.m_gfxShape->m_numIndices,
B3_GL_TRIANGLES,
textureIndex);
delete meshData.m_gfxShape;
delete meshData.m_textureImage;
}
return shapeId;
}

1
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
View File

@@ -15,7 +15,6 @@ struct b3ImportMeshData
class b3ImportMeshUtility
{
public:
static int loadAndRegisterMeshFromFile(const std::string& fileName, class CommonRenderInterface* renderer);
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);

28
examples/Importers/ImportObjDemo/ImportObjExample.cpp
View File

@@ -56,7 +56,31 @@ ImportObjSetup::~ImportObjSetup()
int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer)
{
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
{
int textureIndex = -1;
if (meshData.m_textureImage)
{
textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
}
shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,
&meshData.m_gfxShape->m_indices->at(0),
meshData.m_gfxShape->m_numIndices,
B3_GL_TRIANGLES,
textureIndex);
delete meshData.m_gfxShape;
delete meshData.m_textureImage;
}
return shapeId;
}
@@ -77,7 +101,7 @@ void ImportObjSetup::initPhysics()
btVector3 scaling(1,1,1);
btVector3 color(1,1,1);
int shapeId = b3ImportMeshUtility::loadAndRegisterMeshFromFile(m_fileName, m_guiHelper->getRenderInterface());
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
if (shapeId>=0)
{
//int id =

54
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics()
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
mb = creation.getBulletMultiBody();
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 10.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
}
}

31
examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
View File

@@ -13,9 +13,12 @@
class ImportSTLSetup : public CommonRigidBodyBase
{
const char* m_fileName;
btVector3 m_scaling;
public:
ImportSTLSetup(struct GUIHelperInterface* helper);
ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportSTLSetup();
virtual void initPhysics();
@@ -31,10 +34,19 @@ public:
};
ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
:CommonRigidBodyBase(helper),
m_scaling(btVector3(10,10,10))
{
if (fileName)
{
m_fileName = fileName;
m_scaling = btVector3(0.01,0.01,0.01);
} else
{
m_fileName = "l_finger_tip.stl";
}
}
ImportSTLSetup::~ImportSTLSetup()
@@ -51,17 +63,16 @@ void ImportSTLSetup::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
const char* fileName = "l_finger_tip.stl";
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
{
b3Warning("Cannot find file %s\n", fileName);
b3Warning("Cannot find file %s\n", m_fileName);
return;
}
btVector3 shift(0,0,0);
btVector3 scaling(10,10,10);
// int index=10;
{
@@ -81,12 +92,12 @@ void ImportSTLSetup::initPhysics()
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling);
}
}
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options)
{
return new ImportSTLSetup(options.m_guiHelper);
return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
}

11
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
View File

@@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
for (int i=0;i<numTriangles;i++)
{
char* curPtr = &memoryBuffer[84+i*50];
MySTLTriangle* tri = (MySTLTriangle*) curPtr;
MySTLTriangle tmp;
memcpy(&tmp,curPtr,sizeof(MySTLTriangle));
GLInstanceVertex v0,v1,v2;
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
{
v0.xyzw[v] = tri->vertex0[v];
v1.xyzw[v] = tri->vertex1[v];
v2.xyzw[v] = tri->vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
v0.xyzw[v] = tmp.vertex0[v];
v1.xyzw[v] = tmp.vertex1[v];
v2.xyzw[v] = tmp.vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;

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

@@ -13,7 +13,7 @@ subject to the following restrictions:
#include "BulletUrdfImporter.h"
#include "../../CommonInterfaces/CommonRenderInterface.h"
#include "URDFImporterInterface.h"
#include "btBulletCollisionCommon.h"
@@ -26,12 +26,20 @@ subject to the following restrictions:
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
#include <iostream>
#include <fstream>
#include "UrdfParser.h"
struct MyTexture
{
int m_width;
int m_height;
unsigned char* textureData;
};
struct BulletURDFInternalData
{
UrdfParser m_urdfParser;
@@ -267,14 +275,25 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
//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);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
mass = link->m_inertia.m_mass;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
mass = 0.f;
localInertiaDiagonal.setValue(0,0,0);
}
else
{
mass = link->m_inertia.m_mass;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
inertialFrame = link->m_inertia.m_linkLocalFrame;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
else
{
@@ -578,7 +597,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
}
static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
{
@@ -682,7 +701,23 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
{
case FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
{
if (meshData.m_textureImage)
{
MyTexture texData;
texData.m_width = meshData.m_textureWidth;
texData.m_height = meshData.m_textureHeight;
texData.textureData = meshData.m_textureImage;
texturesOut.push_back(texData);
}
glmesh = meshData.m_gfxShape;
}
break;
}
@@ -903,7 +938,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<MyTexture> textures;
const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr)
@@ -923,14 +959,34 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
}
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices);
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
}
}
if (vertices.size() && indices.size())
{
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
// graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
//graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
if (renderer)
{
int textureIndex = -1;
if (textures.size())
{
textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
}
graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
}
}
//delete textures
for (int i=0;i<textures.size();i++)
{
delete textures[i].textureData;
}
return graphicsIndex;
}

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -128,7 +128,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
gFileNameArray.push_back("sphere2.urdf");
}

93
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -393,13 +393,22 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
geom.m_meshFileName = shape->Attribute("filename");
if (shape->Attribute("scale"))
geom.m_meshScale.setValue(1,1,1);
if (shape->Attribute("scale"))
{
parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
{
logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
std::string scalar_str = shape->Attribute("scale");
double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
if (scaleFactor)
{
geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
}
}
} else
{
geom.m_meshScale.setValue(1,1,1);
}
}
}
@@ -490,29 +499,47 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
// Material
TiXmlElement *mat = config->FirstChildElement("material");
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
if (mat && !m_parseSDF)
if (mat)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
if (m_parseSDF)
{
UrdfMaterial* matPtr = new UrdfMaterial;
matPtr->m_name = "mat";
TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
if (diffuse) {
std::string diffuseText = diffuse->GetText();
btVector4 rgba(1,0,0,1);
parseVector4(rgba,diffuseText);
matPtr->m_rgbaColor = rgba;
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_materialName = "mat";
visual.m_hasLocalMaterial = true;
}
}
else
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
}
}
return true;
@@ -529,6 +556,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
@@ -572,7 +601,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
@@ -1240,6 +1269,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
TiXmlElement* stat = robot_xml->FirstChildElement("static");
if (0!=stat)
{
int val = int(atof(stat->GetText()));
if (val==1)
{
localModel->m_overrideFixedBase = true;
}
}
// Get robot name
const char *name = robot_xml->Attribute("name");

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

@@ -134,6 +134,13 @@ struct UrdfModel
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
:m_overrideFixedBase(false)
{
m_rootTransformInWorld.setIdentity();
}
};

12
examples/InverseDynamics/premake4.lua
View File

@@ -38,7 +38,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
@@ -89,7 +90,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
if os.is("Linux") then initX11() end
@@ -153,7 +155,8 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
if os.is("Linux") then initX11() end
@@ -211,6 +214,7 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}

30
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -102,11 +102,11 @@ float loop;
}
-(void)drawRect:(NSRect)rect
{
if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight))
{
m_lastWidth = [self frame].size.width;
m_lastHeight = [self frame].size.height;
// Only needed on resize:
[m_context clearDrawable];
@@ -114,7 +114,6 @@ float loop;
float width = [self frame].size.width;
float height = [self frame].size.height;
// Get view dimensions in pixels
// glViewport(0,0,10,10);
@@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData
m_myview = 0;
m_pool = 0;
m_window = 0;
m_width = -1;
m_height = -1;
m_exitRequested = false;
}
NSApplication* m_myApp;
TestView* m_myview;
NSAutoreleasePool* m_pool;
NSWindow* m_window;
int m_width;
int m_height;
bool m_exitRequested;
};
@@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
if (m_internalData)
closeWindow();
int width = ci.m_width;
int height = ci.m_height;
const char* windowTitle = ci.m_title;
@@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
m_internalData = new MacOpenGLWindowInternalData;
m_internalData->m_width = width;
m_internalData->m_height = height;
m_internalData->m_pool = [NSAutoreleasePool new];
m_internalData->m_myApp = [NSApplication sharedApplication];
//myApp = [MyApp sharedApplication];
@@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[newItem release];
*/
NSRect frame = NSMakeRect(0., 0., width, height);
NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height);
m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame
@@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[m_internalData->m_window makeKeyAndOrderFront: nil];
[m_internalData->m_myview MakeCurrent];
m_internalData->m_width = m_internalData->m_myview.GetWindowWidth;
m_internalData->m_height = m_internalData->m_myview.GetWindowHeight;
[NSApp activateIgnoringOtherApps:YES];
@@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering()
float aspect;
//b3Vector3 extents;
if (m_internalData->m_width > m_internalData->m_height)
if (getWidth() > getHeight())
{
aspect = (float)m_internalData->m_width / (float)m_internalData->m_height;
aspect = (float)getWidth() / (float)getHeight();
//extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = (float)m_internalData->m_height / (float)m_internalData->m_width;
aspect = (float)getHeight() / (float)getWidth();
//extents.setValue(1.0f, aspect*1.f,0);
}
@@ -1082,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
NSOpenGLContext *foo = [NSOpenGLContext currentContext];
// get the url of a .txt file
NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",nil];
NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil];
[zOpenPanel setAllowedFileTypes:zAryOfExtensions];
NSInteger zIntResult = [zOpenPanel runModal];
@@ -1136,6 +1125,7 @@ int MacOpenGLWindow::getWidth() const
{
if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth)
return m_internalData->m_myview.GetWindowWidth;
return 0;
}
@@ -1152,7 +1142,7 @@ void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
[m_internalData->m_myview setResizeCallback:resizeCallback];
if (resizeCallback)
{
(resizeCallback)(m_internalData->m_width,m_internalData->m_height);
(resizeCallback)(getWidth(), getHeight());
}
}

6
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -147,7 +147,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
m_app->m_renderer->enableBlend(true);
const char* fileName = "teddy.obj";//cube.obj";//textured_sphere_smooth.obj";//cube.obj";
const char* fileName = "textured_sphere_smooth.obj";
fileName = "cube.obj";
{
@@ -181,7 +183,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_guiHelper->getRenderInterface()->writeTransforms();
m_internalData->m_shapePtr.push_back(0);
TinyRenderObjectData* ob = new TinyRenderObjectData(m_internalData->m_width,m_internalData->m_height,
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer);
//ob->loadModel("cube.obj");

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -26,7 +26,7 @@ public:
virtual int getNumJoints(int bodyIndex) const = 0;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

248
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1,10 +1,34 @@
#include "PhysicsClientC_API.h"
#include "PhysicsClientSharedMemory.h"
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Vector3.h"
#include <string.h>
#include "SharedMemoryCommands.h"
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_SDF;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
@@ -127,7 +151,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand
}
b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient,0,controlMode);
}
b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -136,8 +165,12 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -146,6 +179,9 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
return 0;
}
@@ -154,6 +190,9 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
return 0;
}
@@ -162,6 +201,9 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
return 0;
}
@@ -170,6 +212,9 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
return 0;
}
@@ -179,6 +224,9 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -187,6 +235,9 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -335,6 +386,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -343,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -355,6 +412,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
return 0;
}
@@ -368,6 +430,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
return 0;
}
@@ -380,6 +448,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
for (int i=0;i<numJointPositions;i++)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
}
return 0;
}
@@ -396,6 +465,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
{
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
}
return 0;
}
@@ -403,7 +473,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -414,7 +484,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
command->m_createSensorArguments.m_bodyUniqueId = 0;
command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
@@ -489,6 +559,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
return CMD_INVALID_STATUS;
}
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
{
int numBodies = 0;
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
if (status)
{
switch (status->m_type)
{
case CMD_SDF_LOADING_COMPLETED:
{
int i,maxBodies;
numBodies = status->m_sdfLoadedArgs.m_numBodies;
maxBodies = btMin(bodyIndicesCapacity, numBodies);
for (i=0;i<maxBodies;i++)
{
bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
}
break;
}
}
}
return numBodies;
}
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -528,6 +625,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
return false;
if (bodyUniqueId) {
*bodyUniqueId = args.m_bodyUniqueId;
}
@@ -592,10 +693,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
}
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(bodyIndex, linkIndex,*info);
return cl->getJointInfo(bodyIndex, linkIndex,*info);
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
@@ -705,6 +806,94 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
b3Vector3 f = (center - eye).normalized();
b3Vector3 u = up.normalized();
b3Vector3 s = (f.cross(u)).normalized();
u = s.cross(f);
float viewMatrix[16];
viewMatrix[0*4+0] = s.x;
viewMatrix[1*4+0] = s.y;
viewMatrix[2*4+0] = s.z;
viewMatrix[0*4+1] = u.x;
viewMatrix[1*4+1] = u.y;
viewMatrix[2*4+1] = u.z;
viewMatrix[0*4+2] =-f.x;
viewMatrix[1*4+2] =-f.y;
viewMatrix[2*4+2] =-f.z;
viewMatrix[0*4+3] = 0.f;
viewMatrix[1*4+3] = 0.f;
viewMatrix[2*4+3] = 0.f;
viewMatrix[3*4+0] = -s.dot(eye);
viewMatrix[3*4+1] = -u.dot(eye);
viewMatrix[3*4+2] = f.dot(eye);
viewMatrix[3*4+3] = 1.f;
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal)
{
float frustum[16];
frustum[0*4+0] = (float(2) * nearVal) / (right - left);
frustum[0*4+1] = float(0);
frustum[0*4+2] = float(0);
frustum[0*4+3] = float(0);
frustum[1*4+0] = float(0);
frustum[1*4+1] = (float(2) * nearVal) / (top - bottom);
frustum[1*4+2] = float(0);
frustum[1*4+3] = float(0);
frustum[2*4+0] = (right + left) / (right - left);
frustum[2*4+1] = (top + bottom) / (top - bottom);
frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal);
frustum[2*4+3] = float(-1);
frustum[3*4+0] = float(0);
frustum[3*4+1] = float(0);
frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
frustum[3*4+3] = float(0);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_pixelWidth = width;
command->m_requestPixelDataArguments.m_pixelHeight = height;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;
}
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -714,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_APPLY_EXTERNAL_FORCE;
command->m_updateFlags = 0;
command->m_externalForceArguments.m_numForcesAndTorques = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
command->m_externalForceArguments.m_positions[index+i] = position[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}

28
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
@@ -55,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
///Request 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
@@ -68,6 +70,9 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
///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]);
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
@@ -88,14 +93,22 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
///Set joint control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Set joint motor control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
//Only use when controlMode is CONTROL_MODE_VELOCITY
///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
@@ -127,7 +140,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
@@ -146,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
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);
#ifdef __cplusplus
}
#endif

219
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -21,8 +21,8 @@ struct MyMotorInfo2
int m_qIndex;
};
int camVisualizerWidth = 640;//1024/3;
int camVisualizerHeight = 480;//768/3;
int camVisualizerWidth = 320;//1024/3;
int camVisualizerHeight = 240;//768/3;
#define MAX_NUM_MOTORS 128
@@ -37,6 +37,9 @@ protected:
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
btAlignedObjectArray<int> m_bodyUniqueIds;
int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
@@ -66,7 +69,8 @@ protected:
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
int bodyIndex = comboIndex;
int itemIndex = int(atoi(name));
int bodyIndex = m_bodyUniqueIds[itemIndex];
if (m_selectedBody != bodyIndex)
{
m_selectedBody = bodyIndex;
@@ -77,10 +81,10 @@ protected:
virtual void resetCamera()
{
float dist = 5;
float pitch = 50;
float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
float dist = 4;
float pitch = 193;
float yaw = 25;
float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
@@ -154,18 +158,20 @@ protected:
void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
{
for (int i=0;i<m_numMotors;i++)
{
// btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
// int uIndex = m_motorTargetVelocities[i].m_uIndex;
// b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
int qIndex = m_motorTargetPositions[i].m_qIndex;
int uIndex = m_motorTargetPositions[i].m_uIndex;
static int serial=0;
serial++;
// b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos);
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetKp(commandHandle, uIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0.0);
b3JointControlSetKp(commandHandle, qIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
}
@@ -191,7 +197,7 @@ protected:
void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
{
b3Printf("Item selected %s", item);
//b3Printf("Item selected %s", item);
PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
b3Assert(cl);
@@ -227,9 +233,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 0;
@@ -238,7 +242,13 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_LOAD_SDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
@@ -252,7 +262,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -363,11 +374,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
case CMD_SEND_DESIRED_STATE:
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
if (m_selectedBody>=0)
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
// b3Printf("prepare control command for body %d", m_selectedBody);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
break;
}
break;
}
case CMD_RESET_SIMULATION:
{
@@ -430,6 +448,11 @@ PhysicsClientExample::~PhysicsClientExample()
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas && (m_canvasIndex>=0))
{
m_canvas->destroyCanvas(m_canvasIndex);
}
b3Printf("~PhysicsClientExample\n");
}
@@ -450,6 +473,7 @@ void PhysicsClientExample::createButtons()
m_guiHelper->getParameterInterface()->removeAllParameters();
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
@@ -464,9 +488,36 @@ void PhysicsClientExample::createButtons()
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
if (m_bodyUniqueIds.size())
{
if (m_selectedBody<0)
m_selectedBody = 0;
ComboBoxParams comboParams;
comboParams.m_comboboxId = 0;
comboParams.m_numItems = m_bodyUniqueIds.size();
comboParams.m_startItem = m_selectedBody;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
//todo: get the real object name
const char** blarray = new const char*[m_bodyUniqueIds.size()];
for (int i=0;i<m_bodyUniqueIds.size();i++)
{
char* bla = new char[16];
sprintf(bla,"%d", i);
blarray[i] = bla;
comboParams.m_items=blarray;//{&bla};
}
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
if (m_physicsClientHandle && m_selectedBody>=0)
{
m_numMotors = 0;
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
for (int i=0;i<numJoints;i++)
{
@@ -527,7 +578,6 @@ void PhysicsClientExample::initPhysics()
m_selectedBody = -1;
m_prevSelectedBody = -1;
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
@@ -557,13 +607,22 @@ void PhysicsClientExample::initPhysics()
}
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
//m_physicsClientHandle = b3ConnectPhysicsDirect();
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
if (m_options == eCLIENTEXAMPLE_DIRECT)
{
m_physicsClientHandle = b3ConnectPhysicsDirect();
} else
{
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
}
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
@@ -603,48 +662,78 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
// static int counter=0;
// char msg[1024];
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
for (int j=0;j<camVisualizerHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int bytesPerPixel = 4; //RGBA
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,i,j,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255);
// imageData.m_rgbColorData[pixelIndex+3]);
255); //alpha set to 255
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
// b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Printf("Camera image FAILED\n");
b3Warning("Camera image FAILED\n");
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
if (statusType == CMD_SDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
int bodyIndicesOut[1024];
int bodyCapacity = 1024;
int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity);
if (numBodies > bodyCapacity)
{
b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
} else
{
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = bodyIndicesOut[i];
m_bodyUniqueIds.push_back(bodyUniqueId);
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
if (numJoints>0)
{
m_selectedBody = bodyUniqueId;
}
/* int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
b3Printf("numJoints = %d", numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
*/
}
}
//int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
//int bodyIndex = b3GetStatusBodyIndex(status);
/*if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
@@ -653,7 +742,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
@@ -667,12 +755,29 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
*/
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
m_bodyUniqueIds.push_back(bodyIndex);
m_selectedBody = bodyIndex;
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
@@ -695,6 +800,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
m_selectedBody = -1;
m_numMotors=0;
m_bodyUniqueIds.clear();
createButtons();
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
if (m_options == eCLIENTEXAMPLE_SERVER)
@@ -707,8 +813,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
b3Printf("Status after reset: %d",statusType);
//int statusType = b3GetStatusType(status);
//b3Printf("Status after reset: %d",statusType);
}
}
} else
@@ -725,13 +831,12 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
}
}

2
examples/SharedMemory/PhysicsClientExample.h
View File

@@ -4,7 +4,7 @@
enum ClientExampleOptions
{
eCLIENTEXAMPLE_LOOPBACK=1,
eCLIENTEAXMPLE_DIRECT=2,
eCLIENTEXAMPLE_DIRECT=2,
eCLIENTEXAMPLE_SERVER=3,
};

138
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -37,10 +37,13 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus;
int m_counter;
bool m_serverLoadUrdfOK;
bool m_isConnected;
bool m_waitingForServer;
bool m_hasLastServerStatus;
@@ -54,7 +57,6 @@ struct PhysicsClientSharedMemoryInternalData {
m_counter(0),
m_cachedCameraPixelsWidth(0),
m_cachedCameraPixelsHeight(0),
m_serverLoadUrdfOK(false),
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(false),
@@ -83,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
}
void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
return false;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -168,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() {
return true;
}
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
SharedMemoryStatus* stat = 0;
@@ -204,8 +249,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
break;
}
case CMD_SDF_LOADING_COMPLETED: {
if (m_data->m_verboseOutput) {
b3Printf("Server loading the SDF OK\n");
}
break;
}
case CMD_URDF_LOADING_COMPLETED: {
m_data->m_serverLoadUrdfOK = true;
if (m_data->m_verboseOutput) {
b3Printf("Server loading the URDF OK\n");
}
@@ -265,7 +318,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the URDF...\n");
}
m_data->m_serverLoadUrdfOK = false;
break;
}
case CMD_BODY_INFO_COMPLETED:
{
if (m_data->m_verboseOutput) {
b3Printf("Received body info\n");
}
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyUniqueId, serverCmd);
break;
}
case CMD_SDF_LOADING_FAILED: {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the SDF...\n");
}
break;
}
@@ -448,8 +519,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
printf("pixel = %d\n", rgbaPixelsReceived[0]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
@@ -461,7 +539,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CAMERA_IMAGE_FAILED:
{
b3Printf("Camera image FAILED\n");
b3Warning("Camera image FAILED\n");
break;
}
@@ -483,6 +561,50 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_waitingForServer = true;
}
if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)
{
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
for (int i=0;i<numBodies;i++)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
}
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{
//are there any bodies left to be processed?
if (m_data->m_bodyIdsRequestInfo.size())
{
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];

4
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
@@ -34,7 +36,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

205
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
int m_cachedCameraPixelsWidth;
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
PhysicsServerCommandProcessor* m_commandProcessor;
PhysicsDirectInternalData()
@@ -167,6 +173,122 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
if (m_data->m_verboseOutput)
{
b3Printf("Camera image OK\n");
}
int numBytesPerPixel = 4;//RGBA
int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
m_data->m_cachedCameraPixelsWidth = 0;
m_data->m_cachedCameraPixelsHeight = 0;
int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
= rgbaPixelsReceived[i];
}
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
{
// continue requesting remaining pixels
command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
command.m_requestPixelDataArguments.m_startPixelIndex =
serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
} else
{
m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
}
}
} while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
return m_data->m_hasStatus;
}
void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&m_data->m_bulletStreamDataServerToClient[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -174,6 +296,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return processDebugLines(command);
}
if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
{
return processCamera(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
@@ -210,46 +337,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
break;
}
case CMD_SDF_LOADING_COMPLETED:
{
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (hasStatus)
{
processBodyJointInfo(bodyUniqueId, infoStatus);
}
}
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0)
{
bParse::btBulletFile bf(
&m_data->m_bulletStreamDataServerToClient[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyIndex,serverCmd);
}
break;
}
@@ -277,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const
return 0;
}
void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
if (jointIndex < bodyJoints->m_jointInfo.size())
{
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
}
return false;
}
///todo: move this out of the
@@ -333,9 +454,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
{
if (cameraData)
{
cameraData->m_pixelHeight = 0;
cameraData->m_pixelWidth = 0;
cameraData->m_depthValues = 0;
cameraData->m_rgbColorData = 0;
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
}
}

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -19,6 +19,10 @@ protected:
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsDirect();
@@ -44,7 +48,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

4
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const
return m_data->m_physicsClient->getNumJoints(bodyIndex);
}
void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
}
///todo: move this out of the

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -40,7 +40,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

787
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

File diff suppressed because it is too large Load Diff

6
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -19,10 +19,16 @@ class PhysicsServerCommandProcessor
protected:
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
public:
PhysicsServerCommandProcessor();

76
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -30,8 +30,10 @@
#define MAX_DEGREE_OF_FREEDOM 64
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_SDF_BODIES 1024
struct TmpFloat3
{
@@ -54,6 +56,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
return tmp;
}
enum EnumSdfArgsUpdateFlags
{
SDF_ARGS_FILE_NAME=1,
};
struct SdfArgs
{
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
};
enum EnumUrdfArgsUpdateFlags
{
URDF_ARGS_FILE_NAME=1,
@@ -103,6 +115,7 @@ enum EnumInitPoseFlags
struct InitPoseArgs
{
int m_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
};
@@ -118,12 +131,16 @@ struct RequestPixelDataArgs
float m_viewMatrix[16];
float m_projectionMatrix[16];
int m_startPixelIndex;
int m_pixelWidth;
int m_pixelHeight;
};
enum EnumRequestPixelDataUpdateFlags
{
REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL=2,
REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
};
@@ -163,11 +180,14 @@ struct SendDesiredStateArgs
double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
//desired state is only written by the client, read-only access by server is expected
//m_desiredStateQ is indexed by position variables,
//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
@@ -179,6 +199,15 @@ struct SendDesiredStateArgs
};
enum EnumSimDesiredStateUpdateFlags
{
SIM_DESIRED_STATE_HAS_Q=1,
SIM_DESIRED_STATE_HAS_QDOT=2,
SIM_DESIRED_STATE_HAS_KD=4,
SIM_DESIRED_STATE_HAS_KP=8,
SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
};
enum EnumSimParamUpdateFlags
{
@@ -274,6 +303,49 @@ struct CreateBoxShapeArgs
double m_colorRGBA[4];
};
struct SdfLoadedArgs
{
int m_numBodies;
int m_bodyUniqueIds[MAX_SDF_BODIES];
///@todo(erwincoumans) load cameras, lights etc
//int m_numCameras;
//int m_numLights;
};
struct SdfRequestInfoArgs
{
int m_bodyUniqueId;
};
///flags for b3ApplyExternalTorque and b3ApplyExternalForce
enum EnumExternalForcePrivateFlags
{
// EF_LINK_FRAME=1,
// EF_WORLD_FRAME=2,
EF_TORQUE=4,
EF_FORCE=8,
};
struct ExternalForceArgs
{
int m_numForcesAndTorques;
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_linkIds[MAX_SDF_BODIES];
double m_forcesAndTorques[3*MAX_SDF_BODIES];
double m_positions[3*MAX_SDF_BODIES];
int m_forceFlags[MAX_SDF_BODIES];
};
enum EnumSdfRequestInfoFlags
{
SDF_REQUEST_INFO_BODY=1,
//SDF_REQUEST_INFO_CAMERA=2,
};
struct SharedMemoryCommand
{
int m_type;
@@ -287,6 +359,8 @@ struct SharedMemoryCommand
union
{
struct UrdfArgs m_urdfArguments;
struct SdfArgs m_sdfArguments;
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
@@ -297,6 +371,7 @@ struct SharedMemoryCommand
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
struct RequestPixelDataArgs m_requestPixelDataArguments;
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
};
};
@@ -315,6 +390,7 @@ struct SharedMemoryStatus
union
{
struct BulletDataStreamArgs m_dataStreamArguments;
struct SdfLoadedArgs m_sdfLoadedArgs;
struct SendActualStateArgs m_sendActualStateArgs;
struct SendDebugLinesArgs m_sendDebugLinesArgs;
struct SendPixelDataArgs m_sendPixelDataArguments;

16
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
enum EnumSharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_LOAD_SDF,
CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
@@ -18,12 +19,14 @@ enum EnumSharedMemoryClientCommand
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_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION,
CMD_PICK_BODY,
CMD_MOVE_PICKED_BODY,
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_MAX_CLIENT_COMMANDS
};
@@ -35,7 +38,8 @@ enum EnumSharedMemoryServerStatus
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
CMD_SDF_LOADING_COMPLETED,
CMD_SDF_LOADING_FAILED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
@@ -52,6 +56,8 @@ enum EnumSharedMemoryServerStatus
CMD_RESET_SIMULATION_COMPLETED,
CMD_CAMERA_IMAGE_COMPLETED,
CMD_CAMERA_IMAGE_FAILED,
CMD_BODY_INFO_COMPLETED,
CMD_BODY_INFO_FAILED,
CMD_INVALID_STATUS,
CMD_MAX_SERVER_COMMANDS
};
@@ -138,5 +144,11 @@ enum {
CONTROL_MODE_POSITION_VELOCITY_PD,
};
///flags for b3ApplyExternalTorque and b3ApplyExternalForce
enum EnumExternalForceFlags
{
EF_LINK_FRAME=1,
EF_WORLD_FRAME=2,
};
#endif//SHARED_MEMORY_PUBLIC_H

83
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -28,7 +28,7 @@ subject to the following restrictions:
#include "../Utils/b3ResourcePath.h"
#include "../TinyRenderer/TinyRenderer.h"
#include "../OpenGLWindow/SimpleCamera.h"
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include <iostream>
#include <fstream>
#include "../Importers/ImportURDFDemo/UrdfParser.h"
@@ -41,6 +41,13 @@ enum MyFileType
MY_FILE_OBJ=3,
};
struct MyTexture2
{
unsigned char* textureData;
int m_width;
int m_height;
};
struct TinyRendererObjectArray
{
btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects;
@@ -95,7 +102,7 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
{
@@ -201,7 +208,23 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
{
case MY_FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
{
if (meshData.m_textureImage)
{
MyTexture2 texData;
texData.m_width = meshData.m_textureWidth;
texData.m_height = meshData.m_textureHeight;
texData.textureData = meshData.m_textureImage;
texturesOut.push_back(texData);
}
glmesh = meshData.m_gfxShape;
}
break;
}
@@ -420,11 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj)
{
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
int graphicsIndex = -1;
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr)
{
@@ -433,6 +452,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
for (int v = 0; v < link->m_visualArray.size();v++)
{
btAlignedObjectArray<MyTexture2> textures;
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
int graphicsIndex = -1;
const UrdfVisual& vis = link->m_visualArray[v];
btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
@@ -458,14 +483,29 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
btAssert(visualsPtr);
TinyRendererObjectArray* visuals = *visualsPtr;
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices);
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_swWidth,m_data->m_swHeight,m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
unsigned char* textureImage=0;
int textureWidth=0;
int textureHeight=0;
if (textures.size())
{
textureImage = textures[0].textureData;
textureWidth = textures[0].m_width;
textureHeight = textures[0].m_height;
}
tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor,
textureImage,textureWidth,textureHeight);
visuals->m_renderObjects.push_back(tinyObj);
}
for (int i=0;i<textures.size();i++)
{
delete textures[i].textureData;
}
}
}
}
@@ -541,7 +581,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
lightDirWorld.normalize();
printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
// printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
for (int i=0;i<m_data->m_swRenderInstances.size();i++)
{
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
@@ -580,7 +620,9 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
}
}
//printf("write tga \n");
m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
//m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
// printf("flipped!\n");
m_data->m_rgbColorBuffer.flip_vertically();
}
@@ -590,6 +632,17 @@ void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height
height = m_data->m_swHeight;
}
void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
{
m_data->m_swWidth = width;
m_data->m_swHeight = height;
m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
}
void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
{
int w = m_data->m_rgbColorBuffer.get_width();
@@ -612,6 +665,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
{
for (int i=0;i<numRequestedPixels;i++)
{
if (depthBuffer)
{
depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
}
if (pixelsRGBA)
{
pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];

1
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -24,6 +24,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
void resetAll();
void getWidthAndHeight(int& width, int& height);
void setWidthAndHeight(int width, int height);
void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);

5
examples/SharedMemory/premake4.lua
View File

@@ -26,6 +26,8 @@ files {
"PhysicsServer.h",
"main.cpp",
"PhysicsClientC_API.cpp",
"SharedMemoryCommands.h",
"SharedMemoryPublic.h",
"PhysicsServer.cpp",
"PosixSharedMemory.cpp",
"Win32SharedMemory.cpp",
@@ -78,6 +80,7 @@ files {
"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}

6
examples/SimpleOpenGL3/main.cpp
View File

@@ -14,8 +14,8 @@ static b3MouseButtonCallback sOldMouseButtonCB = 0;
static b3KeyboardCallback sOldKeyboardCB = 0;
//static b3RenderCallback sOldRenderCB = 0;
float gWidth = 0 ;
float gHeight = 0;
float gWidth = 1024;
float gHeight = 768;
void MyWheelCallback(float deltax, float deltay)
{
@@ -61,7 +61,7 @@ int main(int argc, char* argv[])
b3CommandLineArgs myArgs(argc,argv);
SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768,true);
SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",gWidth,gHeight,true);
app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);

5
examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
View File

@@ -110,8 +110,9 @@ public:
int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices);
if (shapeIndex>=0)
{
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
float rgbaColor[4] = {1,1,1,1};
swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
m_swRenderObjects.insert(shapeIndex,swObj);
}

5
examples/StandaloneMain/main_tinyrenderer_single_example.cpp
View File

@@ -253,8 +253,9 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
{
int shapeIndex = m_swRenderObjects.size();
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
float rgbaColor[4] = {1,1,1,1};
swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
m_swRenderObjects.insert(shapeIndex,swObj);
return shapeIndex;

38
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -76,21 +76,20 @@ struct Shader : public IShader {
float diff = ambient+b3Min(b3Max(0.f, bn*m_light_dir_local),(1-ambient));
//float diff = b3Max(0.f, n*m_light_dir_local);
color = m_model->diffuse(uv)*diff;
//colors are store in BGRA?
color = TGAColor(color[0]*m_colorRGBA[2],
color[1]*m_colorRGBA[1],
color[2]*m_colorRGBA[0],
color[3]*m_colorRGBA[3]);
//warning: bgra color is swapped to rgba to upload texture
color.bgra[0] *= m_colorRGBA[0];
color.bgra[1] *= m_colorRGBA[1];
color.bgra[2] *= m_colorRGBA[2];
color.bgra[3] *= m_colorRGBA[3];
return false;
}
};
TinyRenderObjectData::TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_width(width),
m_height(height),
m_rgbColorBuffer(rgbColorBuffer),
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_model(0),
m_userData(0),
@@ -102,11 +101,6 @@ m_userIndex(-1)
m_lightDirWorld.setValue(0,0,0);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
m_viewMatrix = lookat(eye, center, up);
//m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
//m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
m_viewportMatrix = viewport(0,0,width,height);
m_projectionMatrix = projection(-1.f/(eye-center).norm());
}
@@ -239,6 +233,9 @@ TinyRenderObjectData::~TinyRenderObjectData()
void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
{
int width = renderData.m_rgbColorBuffer.get_width();
int height = renderData.m_rgbColorBuffer.get_height();
Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
Model* model = renderData.m_model;
if (0==model)
@@ -246,13 +243,8 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
//renderData.m_viewMatrix = lookat(eye, center, up);
int width = renderData.m_width;
int height = renderData.m_height;
//renderData.m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
renderData.m_viewportMatrix = viewport(0,0,renderData.m_width,renderData.m_height);
//renderData.m_projectionMatrix = projection(-1.f/(eye-center).norm());
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
TGAImage& frame = renderData.m_rgbColorBuffer;
@@ -264,7 +256,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
printf("Render %d triangles.\n",model->nfaces());
//printf("Render %d triangles.\n",model->nfaces());
for (int i=0; i<model->nfaces(); i++)
{

5
examples/TinyRenderer/TinyRenderer.h
View File

@@ -25,12 +25,11 @@ struct TinyRenderObjectData
//class IShader* m_shader; todo(erwincoumans) expose the shader, for now we use a default shader
//Output
int m_width;
int m_height;
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;
TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);

2
examples/TinyRenderer/main.cpp
View File

@@ -86,7 +86,7 @@ int main(int argc, char* argv[])
b3AlignedObjectArray<float> depthBuffer;
depthBuffer.resize(gWidth*gHeight);
TinyRenderObjectData renderData(textureWidth, textureHeight,rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
TinyRenderObjectData renderData(rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
//renderData.loadModel("african_head/african_head.obj");
renderData.loadModel("floor.obj");

6
examples/TinyRenderer/model.cpp
View File

@@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid
for (int j=0;j<textureHeight;j++)
{
TGAColor color;
color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
color.bgra[3] = 255;
color.bytespp = 3;

6
examples/Utils/b3ResourcePath.cpp
View File

@@ -77,6 +77,12 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
{
return strlen(resourcePath);
}
sprintf(resourcePath,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
{
return strlen(resourcePath);
}
}
}

14
examples/pybullet/CMakeLists.txt
View File

@@ -9,6 +9,15 @@ INCLUDE_DIRECTORIES(
SET(pybullet_SRCS
pybullet.c
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/SharedMemory/InProcessMemory.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/PhysicsClient.h
@@ -40,6 +49,7 @@ SET(pybullet_SRCS
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
@@ -48,8 +58,8 @@ SET(pybullet_SRCS
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
../../examples/MultiThreading/b3ThreadSupportInterface.cpp

1090
examples/pybullet/pybullet.c
View File

File diff suppressed because it is too large Load Diff