From 5bf125cab525778f5f73395eacfa61fc542486d1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 2 Jun 2016 13:54:52 -0700 Subject: [PATCH 1/9] fix some compile issues using premake use textured cube/sphere model for test --- data/cube.mtl | 2 +- examples/RenderingExamples/TinyRendererSetup.cpp | 4 +++- examples/SharedMemory/PhysicsClientExample.cpp | 7 +++++-- .../main_sw_tinyrenderer_single_example.cpp | 3 ++- .../main_tinyrenderer_single_example.cpp | 3 ++- examples/TinyRenderer/TinyRenderer.cpp | 13 +++++++------ examples/TinyRenderer/model.cpp | 6 +++--- 7 files changed, 23 insertions(+), 15 deletions(-) diff --git a/data/cube.mtl b/data/cube.mtl index fca828974..26b4b44e4 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -10,5 +10,5 @@ newmtl cube Ks 0.0000 0.0000 0.0000 Ke 0.0000 0.0000 0.0000 map_Ka cube.tga - map_Kd floor_diffuse.tga + map_Kd cube.png diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index d7e9f05aa..11258779d 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -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"; { diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e249977bd..a57fde289 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -527,7 +527,6 @@ void PhysicsClientExample::initPhysics() m_selectedBody = -1; m_prevSelectedBody = -1; - if (m_options == eCLIENTEXAMPLE_SERVER) { m_canvas = m_guiHelper->get2dCanvasInterface(); if (m_canvas) @@ -557,9 +556,13 @@ void PhysicsClientExample::initPhysics() } - m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper); } + if (m_options == eCLIENTEXAMPLE_SERVER) + { + m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper); + } + m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); //m_physicsClientHandle = b3ConnectPhysicsDirect(); diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp index 69c3b7db5..786e0eed1 100644 --- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp @@ -111,7 +111,8 @@ public: if (shapeIndex>=0) { TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); - swObj->registerMeshShape(vertices,numvertices,indices,numIndices); + 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); } diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 77ab903ab..157ab233c 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -254,7 +254,8 @@ 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); + 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; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 77c387176..e7dfb8f05 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -76,12 +76,13 @@ 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; } }; diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 626cc2331..f7a0bea75 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid for (int j=0;j Date: Thu, 2 Jun 2016 18:04:22 -0700 Subject: [PATCH 2/9] Support the field under in SDF to make the model immovable, similar to setting the mass to zero. Add joint velocity motors in ImportSDF example. --- .../ImportSDFDemo/ImportSDFSetup.cpp | 54 +++++++++++++++++++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 17 ++++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 14 ++++- .../Importers/ImportURDFDemo/UrdfParser.h | 7 +++ 4 files changed, 88 insertions(+), 4 deletions(-) diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 54883298b..f55cc9758 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -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;iregisterNameForPointer(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_numMotorsc_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++; + } + } + + } + } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 7e9624d64..4838079a0 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -267,14 +267,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 { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 60b40b512..430593b5e 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -529,6 +529,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 +574,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 +1242,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"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1a855f281..b83c0b3c8 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -134,6 +134,13 @@ struct UrdfModel btHashMap m_joints; btArray m_rootLinks; + bool m_overrideFixedBase; + + UrdfModel() + :m_overrideFixedBase(false) + { + m_rootTransformInWorld.setIdentity(); + } }; From 4b2c0f6d8990185691c482e28cbda0c1dd6c59f3 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 3 Jun 2016 19:03:56 -0700 Subject: [PATCH 3/9] make one of the cubes in two_cubes.sdf static (immovable) using the tag in SDF add an example using 'direct' fix the send-desired-state commands, to add flags for arguments set, using default values. Start exposing SDF loading in shared memory api, not fully implemented yet. --- data/two_cubes.sdf | 1 + examples/ExampleBrowser/ExampleEntries.cpp | 5 +- examples/SharedMemory/PhysicsClientC_API.cpp | 30 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../SharedMemory/PhysicsClientExample.cpp | 22 +++-- examples/SharedMemory/PhysicsClientExample.h | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 86 ++++++++++++++++++- examples/SharedMemory/PhysicsDirect.h | 2 + .../PhysicsServerCommandProcessor.cpp | 57 ++++++++---- examples/SharedMemory/SharedMemoryCommands.h | 21 +++++ examples/SharedMemory/SharedMemoryPublic.h | 6 +- 11 files changed, 202 insertions(+), 32 deletions(-) diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf index 24c0854bd..10dce545e 100644 --- a/data/two_cubes.sdf +++ b/data/two_cubes.sdf @@ -86,6 +86,7 @@ 0 + 1 0.512455 -1.58317 0.5 0 -0 0 diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 75e16b8f7..b6004aeef 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -242,7 +242,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 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 31b8df6b9..914c9d091 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -5,6 +5,29 @@ #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 (lenm_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) { @@ -146,6 +169,8 @@ 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; + return 0; } @@ -154,6 +179,7 @@ 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; return 0; } @@ -162,6 +188,8 @@ 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; + return 0; } @@ -170,6 +198,7 @@ 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; return 0; } @@ -179,6 +208,7 @@ 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; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b202daa35..37008aceb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -88,6 +88,8 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); + ///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 b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index a57fde289..15c90748a 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -156,16 +156,12 @@ protected: { for (int i=0;i=0)) + { + m_canvas->destroyCanvas(m_canvasIndex); + } b3Printf("~PhysicsClientExample\n"); } @@ -563,10 +564,15 @@ 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_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"); diff --git a/examples/SharedMemory/PhysicsClientExample.h b/examples/SharedMemory/PhysicsClientExample.h index b564d01be..a36a37e75 100644 --- a/examples/SharedMemory/PhysicsClientExample.h +++ b/examples/SharedMemory/PhysicsClientExample.h @@ -4,7 +4,7 @@ enum ClientExampleOptions { eCLIENTEXAMPLE_LOOPBACK=1, - eCLIENTEAXMPLE_DIRECT=2, + eCLIENTEXAMPLE_DIRECT=2, eCLIENTEXAMPLE_SERVER=3, }; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 72b5eb65e..37baeaa83 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -33,6 +33,12 @@ struct PhysicsDirectInternalData char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + + PhysicsServerCommandProcessor* m_commandProcessor; PhysicsDirectInternalData() @@ -167,6 +173,73 @@ 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]; + printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i=0;im_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; + + +} + bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) @@ -174,6 +247,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) @@ -333,9 +411,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; } } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index fb1aeefa6..9dea925d3 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -19,6 +19,8 @@ protected: bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + bool processCamera(const struct SharedMemoryCommand& orgCommand); + public: PhysicsDirect(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1fc4efdcc..b4317bcc6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1192,12 +1192,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->clearForcesAndTorques(); int torqueIndex = 0; - btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); - btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); + btVector3 f(0,0,0); + btVector3 t(0,0,0); + + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); + t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); + } torqueIndex+=6; mb->addBaseForce(f); mb->addBaseTorque(t); @@ -1206,7 +1212,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int dof=0;dofgetLink(link).m_dofCount;dof++) { - double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + double torque = 0.f; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; mb->addJointTorqueMultiDof(link,dof,torque); torqueIndex++; } @@ -1233,10 +1241,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (motorPtr) { btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0) + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; motor->setVelocityTarget(desiredVelocity); - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); numMotors++; @@ -1247,6 +1260,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } break; } + case CONTROL_MODE_POSITION_VELOCITY_PD: { if (m_data->m_verboseOutput) @@ -1271,11 +1285,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - - btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0) + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + btScalar desiredPosition = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0) + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + + btScalar kp = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0) + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + btScalar kd = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0) + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; int dof1 = 0; btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; @@ -1288,9 +1310,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm motor->setVelocityTarget(desiredVelocity); - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(1000);//maxImp); + motor->setMaxAppliedImpulse(maxImp); numMotors++; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 42436d2b0..8328b485a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -30,6 +30,7 @@ #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 @@ -54,6 +55,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, @@ -179,6 +190,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 { @@ -287,6 +307,7 @@ struct SharedMemoryCommand union { struct UrdfArgs m_urdfArguments; + struct SdfArgs m_sdfArguments; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f7d04f724..2f676e057 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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, @@ -35,7 +36,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, From 612e46614ad1e9b849916a085e69308f5b813e83 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 4 Jun 2016 13:16:06 -0700 Subject: [PATCH 4/9] fix (unused) SSE operator btMatrix3x3 ==, thanks to yyzone for the report/fix. fixes 552 --- src/LinearMath/btMatrix3x3.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 41dea6948..963c5db97 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -1329,7 +1329,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) c0 = _mm_and_ps(c0, c1); c0 = _mm_and_ps(c0, c2); - return (0x7 == _mm_movemask_ps((__m128)c0)); + int m = _mm_movemask_ps((__m128)c0); + return (0x7 == (m & 0x7)); + #else return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && From 8076d5235c52333c655f42adbefa9e054e111bf7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 4 Jun 2016 21:51:21 -0700 Subject: [PATCH 5/9] fix window resize/width bookkeeping bug in MacOpenGLWindow.mm --- examples/OpenGLWindow/MacOpenGLWindow.mm | 28 ++++++++---------------- examples/SimpleOpenGL3/main.cpp | 6 ++--- 2 files changed, 12 insertions(+), 22 deletions(-) diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index 301608cca..d3b3ba31e 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -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); } @@ -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()); } } diff --git a/examples/SimpleOpenGL3/main.cpp b/examples/SimpleOpenGL3/main.cpp index 015db129e..726abf693 100644 --- a/examples/SimpleOpenGL3/main.cpp +++ b/examples/SimpleOpenGL3/main.cpp @@ -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); From 1c7f87aff100b472c2ecc1cb383d9d12556ecd60 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 6 Jun 2016 18:54:05 -0700 Subject: [PATCH 6/9] implement first draft of pybullet.renderImage for synthetic camera remove a few debug printf from tinyrenderer --- .../SharedMemory/PhysicsClientExample.cpp | 14 +- .../PhysicsClientSharedMemory.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 2 +- .../TinyRendererVisualShapeConverter.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 2 +- examples/pybullet/CMakeLists.txt | 10 +- examples/pybullet/pybullet.c | 161 +++++++++++++++++- 7 files changed, 180 insertions(+), 15 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 15c90748a..30da039b3 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -612,9 +612,9 @@ 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) @@ -642,11 +642,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime) 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"); } @@ -716,8 +716,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 diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c0ecc0a19..82062b0c8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -448,7 +448,7 @@ 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]); for (int i=0;im_swRenderInstances.size()); + // printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); for (int i=0;im_swRenderInstances.size();i++) { TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e7dfb8f05..6b9fdbb35 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -265,7 +265,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; infaces(); i++) { diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 5fd6b106a..672187557 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -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 @@ -48,7 +57,6 @@ 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/MultiThreading/b3Win32ThreadSupport.cpp ../../examples/MultiThreading/b3ThreadSupportInterface.cpp diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3286e5cf2..43ac64db9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -134,7 +134,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) float startOrnY = 0; float startOrnZ = 0; float startOrnW = 1; - printf("size=%d\n", size); + //printf("size=%d\n", size); if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -164,7 +164,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); - printf("urdf filename = %s\n", urdfFileName); + //printf("urdf filename = %s\n", urdfFileName); //setting the initial position, orientation and other arguments are optional b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -359,6 +359,157 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) } } +static PyObject* +pybullet_setJointPositions(PyObject* self, PyObject* args) +{ + + Py_INCREF(Py_None); + return Py_None; +} + + // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes + // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats + +static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) +{ + + ///request an image from a simulated camera, using a software renderer. + struct b3CameraImageData imageData; + PyObject* objViewMat,* objProjMat; + + b3SharedMemoryCommandHandle command = b3InitRequestCameraImage(sm); + + if (PyArg_ParseTuple(args, "OO", &objViewMat, &objProjMat)) + { + PyObject* seq; + int i, len; + PyObject* item; + float viewMatrix[16]; + float projectionMatrix[16]; + int valid = 1; + { + seq = PySequence_Fast(objViewMat, "expected a sequence"); + len = PySequence_Size(objViewMat); + //printf("objViewMat size = %d\n", len); + if (len==16) + { + + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + viewMatrix[i] = PyFloat_AsDouble(item); + float v = viewMatrix[i]; + //printf("view %d to %f\n", i,v); + + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + viewMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; + } + } + + { + seq = PySequence_Fast(objProjMat, "expected a sequence"); + len = PySequence_Size(objProjMat); + //printf("projMat len = %d\n", len); + if (len==16) + { + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; + } + } + + + Py_DECREF(seq); + if (valid) + { + //printf("set b3RequestCameraImageSetCameraMatrices\n"); + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); + } + } + + if (b3CanSubmitCommand(sm)) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_CAMERA_IMAGE_COMPLETED) + { + PyObject *item2; + PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth + + b3GetCameraImageData(sm, &imageData); + //todo: error handling if image size is 0 + pyResultList = PyTuple_New(4); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + + PyObject *pylistPos; + PyObject* pylistDep; + + //printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight); + { + + PyObject *item; + int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values + int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; + pylistPos = PyTuple_New(num); + pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); + + for (int i=0;i Date: Tue, 7 Jun 2016 12:57:46 -0700 Subject: [PATCH 7/9] add .stl file support in 'File/Open' of example browser. --- examples/ExampleBrowser/main.cpp | 3 ++ .../ImportSTLDemo/ImportSTLSetup.cpp | 31 +++++++++++++------ examples/OpenGLWindow/MacOpenGLWindow.mm | 2 +- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 179f18036..b245cdecb 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -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) diff --git a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp index b7f25c3dc..c05138ef1 100644 --- a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp +++ b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp @@ -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); } diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index d3b3ba31e..f4f853889 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -1071,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]; From d2e50d045b6c1d236115cf9b5c13558991901df0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 7 Jun 2016 16:11:58 -0700 Subject: [PATCH 8/9] fix issues related to camera width/height add width,height as arguments to pybullet.renderImage(x,y,[viewMat4x4],[projMat4x4]) --- .../RenderingExamples/TinyRendererSetup.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 10 + examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsClientExample.cpp | 23 +- .../PhysicsClientSharedMemory.cpp | 7 + examples/SharedMemory/PhysicsDirect.cpp | 10 +- .../PhysicsServerCommandProcessor.cpp | 9 + examples/SharedMemory/SharedMemoryCommands.h | 4 + .../TinyRendererVisualShapeConverter.cpp | 19 +- .../TinyRendererVisualShapeConverter.h | 1 + examples/TinyRenderer/TinyRenderer.cpp | 22 +- examples/TinyRenderer/TinyRenderer.h | 7 +- examples/pybullet/pybullet.c | 227 ++++++++++-------- 13 files changed, 203 insertions(+), 141 deletions(-) diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 11258779d..8df1a76ad 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -183,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"); @@ -363,7 +363,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime) } } - TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]); + TinyRenderer::renderObject(*m_internalData->m_renderObjects[o], m_internalData->m_width,m_internalData->m_height); } //m_app->drawText("hello",500,500); render->activateTexture(m_internalData->m_textureHandle); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 914c9d091..c6468cb89 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -735,6 +735,16 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa 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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 37008aceb..b67f078ae 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -68,6 +68,7 @@ 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 b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 30da039b3..e7e02ba51 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -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 @@ -77,10 +77,10 @@ protected: virtual void resetCamera() { - float dist = 5; + float dist = 1.1; float pitch = 50; float yaw = 35; - float targetPos[3]={0,0,0};//-3,2.8,-2.5}; + float targetPos[3]={0,0,0.5};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } @@ -248,7 +248,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: @@ -619,18 +620,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3GetCameraImageData(m_physicsClientHandle,&imageData); if (m_canvas && m_canvasIndex >=0) { - for (int i=0;isetPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex, + int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel; + m_canvas->setPixel(m_canvasIndex,i,camVisualizerHeight-1-j, imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 82062b0c8..728358b93 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -450,6 +450,13 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[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;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 37baeaa83..478e31031 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -210,8 +210,15 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) unsigned char* rgbaPixelsReceived = (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; - printf("pixel = %d\n", rgbaPixelsReceived[0]); + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] @@ -227,6 +234,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) 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; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2f662aa4a..89490caa6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -972,6 +972,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int width, height; int numPixelsCopied = 0; + if ( + (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) + { + m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, + clientCmd.m_requestPixelDataArguments.m_pixelHeight); + } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0); @@ -1005,6 +1013,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { // printf("-------------------------------\nRendering\n"); + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) { m_data->m_visualConverter.render( diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8328b485a..fa67d7673 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -129,12 +129,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, + }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index fed633c1b..7c3507b25 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -462,7 +462,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const 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); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer); tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor); visuals->m_renderObjects.push_back(tinyObj); } @@ -576,7 +576,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_lightDirWorld = lightDirWorld; } } - TinyRenderer::renderObject(*renderObj); + TinyRenderer::renderObject(*renderObj,m_data->m_swWidth,m_data->m_swHeight); } } //printf("write tga \n"); @@ -590,6 +590,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 +623,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels { for (int i=0;im_depthBuffer[i+startPixelIndex]; + } if (pixelsRGBA) { pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0]; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 3bce1add7..6baff2491 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -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); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 6b9fdbb35..e13f97077 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -88,10 +88,8 @@ struct Shader : public IShader { }; -TinyRenderObjectData::TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) -:m_width(width), -m_height(height), -m_rgbColorBuffer(rgbColorBuffer), +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) +:m_rgbColorBuffer(rgbColorBuffer), m_depthBuffer(depthBuffer), m_model(0), m_userData(0), @@ -103,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()); } @@ -238,7 +231,7 @@ TinyRenderObjectData::~TinyRenderObjectData() delete m_model; } -void TinyRenderer::renderObject(TinyRenderObjectData& renderData) +void TinyRenderer::renderObject(TinyRenderObjectData& renderData, int width, int height) { Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]); Model* model = renderData.m_model; @@ -247,13 +240,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& zbuffer = renderData.m_depthBuffer; TGAImage& frame = renderData.m_rgbColorBuffer; diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 0cb2ffca9..cfe857eca 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -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& m_depthBuffer; - TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); virtual ~TinyRenderObjectData(); void loadModel(const char* fileName); @@ -48,7 +47,7 @@ struct TinyRenderObjectData class TinyRenderer { public: - static void renderObject(TinyRenderObjectData& renderData); + static void renderObject(TinyRenderObjectData& renderData, int width, int height); }; #endif // TINY_RENDERER_Hbla diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 43ac64db9..0bac58313 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -362,6 +362,11 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) static PyObject* pybullet_setJointPositions(PyObject* self, PyObject* args) { + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } Py_INCREF(Py_None); return Py_None; @@ -372,17 +377,22 @@ pybullet_setJointPositions(PyObject* self, PyObject* args) static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { - + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + ///request an image from a simulated camera, using a software renderer. struct b3CameraImageData imageData; PyObject* objViewMat,* objProjMat; + int width, height; - b3SharedMemoryCommandHandle command = b3InitRequestCameraImage(sm); - - if (PyArg_ParseTuple(args, "OO", &objViewMat, &objProjMat)) + if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) { - PyObject* seq; - int i, len; + + PyObject* seq; + int i, len; PyObject* item; float viewMatrix[16]; float projectionMatrix[16]; @@ -391,124 +401,133 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) seq = PySequence_Fast(objViewMat, "expected a sequence"); len = PySequence_Size(objViewMat); //printf("objViewMat size = %d\n", len); - if (len==16) - { - - if (PyList_Check(seq)) - { - for (i = 0; i < len; i++) + if (len==16) { + + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { item = PyList_GET_ITEM(seq, i); - viewMatrix[i] = PyFloat_AsDouble(item); - float v = viewMatrix[i]; + viewMatrix[i] = PyFloat_AsDouble(item); + float v = viewMatrix[i]; //printf("view %d to %f\n", i,v); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + viewMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; } } - else - { - for (i = 0; i < len; i++) - { - item = PyTuple_GET_ITEM(seq,i); - viewMatrix[i] = PyFloat_AsDouble(item); - } - } - } else - { - valid = 0; - } - } + { - seq = PySequence_Fast(objProjMat, "expected a sequence"); - len = PySequence_Size(objProjMat); - //printf("projMat len = %d\n", len); - if (len==16) - { - if (PyList_Check(seq)) - { - for (i = 0; i < len; i++) - { - item = PyList_GET_ITEM(seq, i); - projectionMatrix[i] = PyFloat_AsDouble(item); - } - } - else - { - for (i = 0; i < len; i++) - { - item = PyTuple_GET_ITEM(seq,i); - projectionMatrix[i] = PyFloat_AsDouble(item); - } - } - } else - { - valid = 0; - } - } - + seq = PySequence_Fast(objProjMat, "expected a sequence"); + len = PySequence_Size(objProjMat); + //printf("projMat len = %d\n", len); + if (len==16) + { + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; + } + } Py_DECREF(seq); if (valid) { + b3SharedMemoryCommandHandle command; + + command = b3InitRequestCameraImage(sm); + //printf("set b3RequestCameraImageSetCameraMatrices\n"); b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); - } - } - - if (b3CanSubmitCommand(sm)) - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_CAMERA_IMAGE_COMPLETED) - { - PyObject *item2; - PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth - - b3GetCameraImageData(sm, &imageData); - //todo: error handling if image size is 0 - pyResultList = PyTuple_New(4); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - - PyObject *pylistPos; - PyObject* pylistDep; - - //printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight); - { - - PyObject *item; - int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values - int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; - pylistPos = PyTuple_New(num); - pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); - - for (int i=0;i Date: Tue, 7 Jun 2016 17:02:47 -0700 Subject: [PATCH 9/9] don't pass width/height into renderObject --- examples/RenderingExamples/TinyRendererSetup.cpp | 2 +- examples/SharedMemory/TinyRendererVisualShapeConverter.cpp | 2 +- .../StandaloneMain/main_sw_tinyrenderer_single_example.cpp | 2 +- examples/StandaloneMain/main_tinyrenderer_single_example.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 5 ++++- examples/TinyRenderer/TinyRenderer.h | 2 +- examples/TinyRenderer/main.cpp | 2 +- 7 files changed, 10 insertions(+), 7 deletions(-) diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 8df1a76ad..fd3f80e41 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -363,7 +363,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime) } } - TinyRenderer::renderObject(*m_internalData->m_renderObjects[o], m_internalData->m_width,m_internalData->m_height); + TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]); } //m_app->drawText("hello",500,500); render->activateTexture(m_internalData->m_textureHandle); diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 7c3507b25..3bb5e893e 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -576,7 +576,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_lightDirWorld = lightDirWorld; } } - TinyRenderer::renderObject(*renderObj,m_data->m_swWidth,m_data->m_swHeight); + TinyRenderer::renderObject(*renderObj); } } //printf("write tga \n"); diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp index 786e0eed1..1fa1b0ccd 100644 --- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp @@ -110,7 +110,7 @@ public: int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices); if (shapeIndex>=0) { - TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); + 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); diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 157ab233c..565b771e7 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -253,7 +253,7 @@ struct TinyRendererGUIHelper : public GUIHelperInterface { int shapeIndex = m_swRenderObjects.size(); - TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); + 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); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e13f97077..aebf0440e 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -231,8 +231,11 @@ TinyRenderObjectData::~TinyRenderObjectData() delete m_model; } -void TinyRenderer::renderObject(TinyRenderObjectData& renderData, int width, int height) +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) diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index cfe857eca..c6f4f9821 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -47,7 +47,7 @@ struct TinyRenderObjectData class TinyRenderer { public: - static void renderObject(TinyRenderObjectData& renderData, int width, int height); + static void renderObject(TinyRenderObjectData& renderData); }; #endif // TINY_RENDERER_Hbla diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp index 9995b8f66..32f5e7edd 100644 --- a/examples/TinyRenderer/main.cpp +++ b/examples/TinyRenderer/main.cpp @@ -86,7 +86,7 @@ int main(int argc, char* argv[]) b3AlignedObjectArray 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");