From f80f0c76a4b89b8512dfcfad1a5069ba27da9d06 Mon Sep 17 00:00:00 2001 From: Max Argus Date: Mon, 2 Jul 2018 19:35:55 +0200 Subject: [PATCH] added egl plugin commit --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 4 +- examples/OpenGLWindow/EGLOpenGLWindow.cpp | 7 +- examples/SharedMemory/b3PluginManager.cpp | 6 + .../SharedMemory/plugins/eglPlugin/bullet.py | 25 + .../plugins/eglPlugin/eglRendererPlugin.cpp | 62 + .../plugins/eglPlugin/eglRendererPlugin.h | 25 + .../eglRendererVisualShapeConverter.cpp | 1312 +++++++++++++++++ .../eglRendererVisualShapeConverter.h | 63 + .../plugins/eglPlugin/premake4.lua | 53 + setup.py | 50 +- 10 files changed, 1603 insertions(+), 4 deletions(-) create mode 100644 examples/SharedMemory/plugins/eglPlugin/bullet.py create mode 100644 examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp create mode 100644 examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h create mode 100644 examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp create mode 100644 examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h create mode 100644 examples/SharedMemory/plugins/eglPlugin/premake4.lua diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 07094414c..911654d67 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1367,7 +1367,9 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); if (linkPtr) { - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId); + //(gdb) p *model.m_links.m_valueArray.m_data[0] + std::cout << "mvp " << (*linkPtr)->m_visualArray.size() << std::endl; + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId); } } } diff --git a/examples/OpenGLWindow/EGLOpenGLWindow.cpp b/examples/OpenGLWindow/EGLOpenGLWindow.cpp index a3dbf71c3..14989a520 100644 --- a/examples/OpenGLWindow/EGLOpenGLWindow.cpp +++ b/examples/OpenGLWindow/EGLOpenGLWindow.cpp @@ -36,8 +36,11 @@ #include #include -#include -#include +#include "OpenGLInclude.h" + +#include "EGL/egl.h" +#include "EGL/eglext.h" +#include "GL/gl.h" #include "EGLOpenGLWindow.h" diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 52f7b05b8..17fc25c0f 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -220,7 +220,13 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) } else { + b3Warning("Warning: couldn't load plugin %s\n", pluginPath); + #ifdef _WIN32 + #else + b3Warning("Error: %s\n", dlerror() ); + #endif + } if (!ok) { diff --git a/examples/SharedMemory/plugins/eglPlugin/bullet.py b/examples/SharedMemory/plugins/eglPlugin/bullet.py new file mode 100644 index 000000000..08e1fba57 --- /dev/null +++ b/examples/SharedMemory/plugins/eglPlugin/bullet.py @@ -0,0 +1,25 @@ +import os +import time +import subprocess +import pybullet as p +#subprocess.call(["hardening-check", p.__file__]) + +p.connect(p.DIRECT) + +plugin_fn = '/home/argusm/lang/bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so' +plugin = p.loadPlugin(plugin_fn,"_tinyRendererPlugin") +print("plugin =",plugin) + +path = '/home/argusm/lang/bullet3/examples/pybullet/gym/pybullet_data/r2d2.urdf' +path = '/home/argusm/lang/gym-grasping/gym_grasping/robots/models/kuka_iiwa/kuka_weiss_bolt.sdf' +#p.loadURDF(path) +p.loadSDF(path) + +start = time.time() +for i in range(2000): + p.getCameraImage(80,80) + if i % 100 == 0 and i > 0: + print("FPS",100/(time.time()-start)) + start = time.time() + + diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp new file mode 100644 index 000000000..b90ae3c2c --- /dev/null +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp @@ -0,0 +1,62 @@ + +//tinyRenderer plugin + +/* +import pybullet as p +p.connect(p.GUI) +plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_tinyRendererPlugin_vs2010_x64_debug.dll","_tinyRendererPlugin") +print("plugin=",plugin) +p.loadURDF("r2d2.urdf") +while (1): + p.getCameraImage(320,200) +*/ + +#include "eglRendererPlugin.h" +#include "eglRendererVisualShapeConverter.h" + +#include "../../SharedMemoryPublic.h" +#include "../b3PluginContext.h" +#include + + + +struct MyRendererPluginClass +{ + + TinyRendererVisualShapeConverter m_renderer; + MyRendererPluginClass() + { + } + virtual ~MyRendererPluginClass() + { + } +}; + +B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context) +{ + MyRendererPluginClass* obj = new MyRendererPluginClass(); + context->m_userPointer = obj; + return SHARED_MEMORY_MAGIC_NUMBER; +} + + +B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +{ + return -1; +} + + +B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context) +{ + MyRendererPluginClass* obj = (MyRendererPluginClass*) context->m_userPointer; + delete obj; + context->m_userPointer = 0; +} + +//all the APIs below are optional +B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context) +{ + MyRendererPluginClass* obj = (MyRendererPluginClass*) context->m_userPointer; + return &obj->m_renderer; +} + diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h new file mode 100644 index 000000000..9f1d9330a --- /dev/null +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h @@ -0,0 +1,25 @@ +#ifndef TINY_RENDERER_PLUGIN_H +#define TINY_RENDERER_PLUGIN_H + +#include "../b3PluginAPI.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load +B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context); +B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context); +B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments); + +//all the APIs below are optional +B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context); + + + +#ifdef __cplusplus +}; +#endif + +#endif//#define TEST_PLUGIN_H diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp new file mode 100644 index 000000000..bba78407b --- /dev/null +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -0,0 +1,1312 @@ +/* Copyright (C) 2016 Google + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "eglRendererVisualShapeConverter.h" + + + +#include "../Importers/ImportURDFDemo/URDFImporterInterface.h" +#include "btBulletCollisionCommon.h" +#include "../Importers/ImportObjDemo/LoadMeshFromObj.h" +#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h" +#include "../Importers/ImportColladaDemo/LoadMeshFromCollada.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "Bullet3Common/b3FileUtils.h" +#include +#include "../Utils/b3ResourcePath.h" +#include "../TinyRenderer/TinyRenderer.h" +#include "../OpenGLWindow/SimpleCamera.h" +#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" +#include +#include +#include "../Importers/ImportURDFDemo/UrdfParser.h" +#include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData +#include "../TinyRenderer/model.h" +#include "stb_image/stb_image.h" + + +#include "../OpenGLWindow/EGLOpenGLWindow.h" +#include "../OpenGLWindow/GLInstancingRenderer.h" +#include "../OpenGLWindow/GLRenderToTexture.h" + +static void printGLString(const char *name, GLenum s) { + const char *v = (const char *) glGetString(s); + printf("%s = %s\n",name, v); +} + + +struct MyTexture2 +{ + unsigned char* textureData1; + int m_width; + int m_height; + bool m_isCached; +}; + +struct TinyRendererObjectArray +{ + btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects; + int m_objectUniqueId; + int m_linkIndex; + btTransform m_worldTransform; + btVector3 m_localScaling; + + TinyRendererObjectArray() + { + m_worldTransform.setIdentity(); + m_localScaling.setValue(1,1,1); + } +}; + +#define START_WIDTH 640 +#define START_HEIGHT 480 + +struct TinyRendererVisualShapeConverterInternalData +{ + class CommonWindowInterface* m_window; + class GLInstancingRenderer* m_instancingRenderer; + btHashMap m_swRenderInstances; + + btAlignedObjectArray m_visualShapes; + + int m_upAxis; + int m_swWidth; + int m_swHeight; + TGAImage m_rgbColorBuffer; + b3AlignedObjectArray m_textures; + b3AlignedObjectArray m_depthBuffer; + b3AlignedObjectArray m_shadowBuffer; + b3AlignedObjectArray m_segmentationMaskBuffer; + btVector3 m_lightDirection; + bool m_hasLightDirection; + btVector3 m_lightColor; + bool m_hasLightColor; + float m_lightDistance; + bool m_hasLightDistance; + float m_lightAmbientCoeff; + bool m_hasLightAmbientCoeff; + float m_lightDiffuseCoeff; + bool m_hasLightDiffuseCoeff; + float m_lightSpecularCoeff; + bool m_hasLightSpecularCoeff; + bool m_hasShadow; + int m_flags; + SimpleCamera m_camera; + + + TinyRendererVisualShapeConverterInternalData() + :m_upAxis(2), + m_swWidth(START_WIDTH), + m_swHeight(START_HEIGHT), + m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB), + m_lightDirection(btVector3(-5,200,-40)), + m_hasLightDirection(false), + m_lightColor(btVector3(1.0,1.0,1.0)), + m_hasLightColor(false), + m_lightDistance(2.0), + m_hasLightDistance(false), + m_lightAmbientCoeff(0.6), + m_hasLightAmbientCoeff(false), + m_lightDiffuseCoeff(0.35), + m_hasLightDiffuseCoeff(false), + m_lightSpecularCoeff(0.05), + m_hasLightSpecularCoeff(false), + m_hasShadow(false), + m_flags(0) + { + m_depthBuffer.resize(m_swWidth*m_swHeight); + m_shadowBuffer.resize(m_swWidth*m_swHeight); + m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1); + + // OpenGL window + bool allowRetina=true; + m_window = new EGLOpenGLWindow(); + m_window->setAllowRetina(allowRetina); + b3gWindowConstructionInfo ci; + ci.m_title = "Title"; + ci.m_width = m_swWidth; + ci.m_height = m_swHeight; + m_window->createWindow(ci); + m_window->setWindowTitle(ci.m_title); + b3Assert(glGetError() ==GL_NO_ERROR); + { + printGLString("Version", GL_VERSION); + printGLString("Vendor", GL_VENDOR); + printGLString("Renderer", GL_RENDERER); + } + glClearColor(.7f, .7f, .8f, 1.f); + + m_window->startRendering(); + + b3Assert(glGetError() ==GL_NO_ERROR); + + + glGetError();//don't remove this call, it is needed for Ubuntu + b3Assert(glGetError() ==GL_NO_ERROR); + int maxNumObjectCapacity = 128 * 1024; + int maxShapeCapacityInBytes = 128 * 1024 * 1024; + m_instancingRenderer = new GLInstancingRenderer(maxNumObjectCapacity, maxShapeCapacityInBytes); + m_instancingRenderer->init(); + m_instancingRenderer->resize(m_swWidth,m_swHeight); + m_instancingRenderer->InitShaders(); + } + +}; + + + +TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter() +{ + m_data = new TinyRendererVisualShapeConverterInternalData(); + + float dist = 1.5; + float pitch = -10; + float yaw = -80; + float targetPos[3]={0,0,0}; + m_data->m_camera.setCameraUpAxis(m_data->m_upAxis); + resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]); + + +} +TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter() +{ + resetAll(); + delete m_data; +} + +void TinyRendererVisualShapeConverter::setLightDirection(float x, float y, float z) +{ + m_data->m_lightDirection.setValue(x, y, z); + m_data->m_hasLightDirection = true; +} + +void TinyRendererVisualShapeConverter::setLightColor(float x, float y, float z) +{ + m_data->m_lightColor.setValue(x, y, z); + m_data->m_hasLightColor = true; +} + +void TinyRendererVisualShapeConverter::setLightDistance(float dist) +{ + m_data->m_lightDistance = dist; + m_data->m_hasLightDistance = true; +} + +void TinyRendererVisualShapeConverter::setShadow(bool hasShadow) +{ + m_data->m_hasShadow = hasShadow; +} +void TinyRendererVisualShapeConverter::setFlags(int flags) +{ + m_data->m_flags = flags; +} + + +void TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff) +{ + m_data->m_lightAmbientCoeff = ambientCoeff; + m_data->m_hasLightAmbientCoeff = true; +} + +void TinyRendererVisualShapeConverter::setLightDiffuseCoeff(float diffuseCoeff) +{ + m_data->m_lightDiffuseCoeff = diffuseCoeff; + m_data->m_hasLightDiffuseCoeff = true; +} + +void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff) +{ + m_data->m_lightSpecularCoeff = specularCoeff; + m_data->m_hasLightSpecularCoeff = true; +} + +void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) +{ + + visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; + visualShapeOut.m_dimensions[0] = 0; + visualShapeOut.m_dimensions[1] = 0; + visualShapeOut.m_dimensions[2] = 0; + memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName)); + if (visual->m_geometry.m_hasLocalMaterial) { + visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0]; + visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[1]; + visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2]; + visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3]; + } + + GLInstanceGraphicsShape* glmesh = 0; + + btConvexShape* convexColShape = 0; + + switch (visual->m_geometry.m_type) + { + case URDF_GEOM_CYLINDER: + case URDF_GEOM_CAPSULE: + { + btVector3 p1 = visual->m_geometry.m_capsuleFrom; + btVector3 p2 = visual->m_geometry.m_capsuleTo; + btTransform tr; + tr.setIdentity(); + btScalar rad, len; + btVector3 center(0,0,0); + btVector3 axis(0,0,1); + btAlignedObjectArray vertices; + int numSteps = 32; + + if (visual->m_geometry.m_hasFromTo) + { + btVector3 v = p2 - p1; + btVector3 dir = v.normalized(); + tr = visual->m_linkLocalFrame; + len = v.length(); + rad = visual->m_geometry.m_capsuleRadius; + btVector3 ax1,ax2; + btPlaneSpace1(dir,ax1,ax2); + + for (int i = 0; im_geometry.m_type==URDF_GEOM_CAPSULE) + { + btVector3 pole1 = p1 - dir * rad; + btVector3 pole2 = p2 + dir * rad; + vertices.push_back(pole1); + vertices.push_back(pole2); + } + + } else { + //assume a capsule along the Z-axis, centered at the origin + tr = visual->m_linkLocalFrame; + len = visual->m_geometry.m_capsuleHeight; + rad = visual->m_geometry.m_capsuleRadius; + for (int i = 0; im_geometry.m_type==URDF_GEOM_CAPSULE) + { + btVector3 pole1(0, 0, + len / 2. + rad); + btVector3 pole2(0, 0, - len / 2. - rad); + vertices.push_back(pole1); + vertices.push_back(pole2); + } + } + visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; + visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1]; + visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2]; + visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0]; + visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1]; + visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2]; + visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; + visualShapeOut.m_dimensions[0] = len; + visualShapeOut.m_dimensions[1] = rad; + + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + //btCapsuleShape* cylZShape = new btCapsuleShape(rad,len);//btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + + cylZShape->setMargin(0.001); + convexColShape = cylZShape; + break; + } + case URDF_GEOM_BOX: + { + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_boxSize[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_boxSize[2]; + + btVector3 extents = visual->m_geometry.m_boxSize; + + btBoxShape* boxShape = new btBoxShape(extents*0.5f); + //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); + convexColShape = boxShape; + convexColShape->setMargin(0.001); + break; + } + case URDF_GEOM_SPHERE: + { + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; + + btScalar radius = visual->m_geometry.m_sphereRadius; + btSphereShape* sphereShape = new btSphereShape(radius); + convexColShape = sphereShape; + convexColShape->setMargin(0.001); + break; + } + case URDF_GEOM_MESH: + { + strncpy(visualShapeOut.m_meshAssetFileName, visual->m_geometry.m_meshFileName.c_str(), VISUAL_SHAPE_MAX_PATH_LEN); + visualShapeOut.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN-1] = 0; + + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; + + switch (visual->m_geometry.m_meshFileType) + { + case UrdfGeometry::FILE_OBJ: + { + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) + { + + if (meshData.m_textureImage1) + { + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData1 = meshData.m_textureImage1; + texData.m_isCached = meshData.m_isCached; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + break; + } + case UrdfGeometry::FILE_STL: + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + break; + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); +// upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + + for (int v = 0; vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str()); + break; + } + + default: + // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) + btAssert(0); + } + + if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) + { + //apply the geometry scaling + for (int i=0;im_vertices->size();i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; + } + } + else + { + b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + } + break; + } // case mesh + + case URDF_GEOM_PLANE: + // TODO: plane in tiny renderer + // TODO: export visualShapeOut for external render + break; + + default: + { + b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); + } + } + + //if we have a convex, tesselate into localVertices/localIndices + if ((glmesh==0) && convexColShape) + { + btShapeHull* hull = new btShapeHull(convexColShape); + hull->buildHull(0.0); + { + // int strideInBytes = 9*sizeof(float); + int numVertices = hull->numVertices(); + int numIndices = hull->numIndices(); + + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + + for (int i = 0; i < numVertices; i++) + { + GLInstanceVertex vtx; + btVector3 pos = hull->getVertexPointer()[i]; + vtx.xyzw[0] = pos.x(); + vtx.xyzw[1] = pos.y(); + vtx.xyzw[2] = pos.z(); + vtx.xyzw[3] = 1.f; + btVector3 normal = pos.safeNormalize(); + vtx.normal[0] = normal.x(); + vtx.normal[1] = normal.y(); + vtx.normal[2] = normal.z(); + btScalar u = btAtan2(normal[0], normal[2]) / (2 * SIMD_PI) + 0.5; + btScalar v = normal[1] * 0.5 + 0.5; + vtx.uv[0] = u; + vtx.uv[1] = v; + glmesh->m_vertices->push_back(vtx); + } + + btAlignedObjectArray indices; + for (int i = 0; i < numIndices; i++) + { + glmesh->m_indices->push_back(hull->getIndexPointer()[i]); + } + + glmesh->m_numvertices = glmesh->m_vertices->size(); + glmesh->m_numIndices = glmesh->m_indices->size(); + } + delete hull; + delete convexColShape; + convexColShape = 0; + + } + + if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) + { + + int baseIndex = verticesOut.size(); + + + + for (int i = 0; i < glmesh->m_indices->size(); i++) + { + indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); + } + + for (int i = 0; i < glmesh->m_vertices->size(); i++) + { + GLInstanceVertex& v = glmesh->m_vertices->at(i); + btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); + btVector3 vt = visualTransform*vert; + v.xyzw[0] = vt[0]; + v.xyzw[1] = vt[1]; + v.xyzw[2] = vt[2]; + btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); + triNormal = visualTransform.getBasis()*triNormal; + v.normal[0] = triNormal[0]; + v.normal[1] = triNormal[1]; + v.normal[2] = triNormal[2]; + verticesOut.push_back(v); + } + } + delete glmesh; + +} + +static btVector4 sColors[4] = +{ + btVector4(60./256.,186./256.,84./256.,1), + btVector4(244./256.,194./256.,13./256.,1), + btVector4(219./256.,50./256.,54./256.,1), + btVector4(72./256.,133./256.,237./256.,1), + + //btVector4(1,1,0,1), +}; + + +void TinyRendererVisualShapeConverter::convertVisualShapes( + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + const UrdfLink* linkPtr, const UrdfModel* model, + int collisionObjectUniqueId, int bodyUniqueId) +{ + btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) + if (linkPtr) + { + bool useVisual; + int cnt = 0; + + std::cout << "mvp " << linkPtr->m_visualArray.size() << std::endl; + + if (linkPtr->m_visualArray.size() > 0) + { + useVisual = true; + cnt = linkPtr->m_visualArray.size(); + } + else + { + // We have to see something, take collision shape. Useful for MuJoCo xml, where there are no explicit visual shapes. + useVisual = false; + cnt = linkPtr->m_collisionArray.size(); + } + + for (int v1=0; v1 textures; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + //int graphicsIndex = -1; + + const UrdfShape* vis; + if (useVisual) { + vis = &linkPtr->m_visualArray[v1]; + } else { + vis = &linkPtr->m_collisionArray[v1]; + } + btTransform childTrans = vis->m_linkLocalFrame; + + int colorIndex = linkIndex;//colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0; + if (colorIndex<0) + colorIndex=0; + colorIndex &=3; + btVector4 color; + color = sColors[colorIndex]; + float rgbaColor[4] = {(float)color[0],(float)color[1],(float)color[2],(float)color[3]}; + //if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) + //{ + // color.setValue(1,1,1,1); + //} + if (model) + { + if (useVisual) + { + btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); + UrdfMaterial*const* matPtr = model->m_materials[matName]; + if (matPtr) + { + for (int i = 0; i < 4; i++) + { + rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i]; + } + //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); + } else + { + ///programmatic created models may have the color in the visual + if (vis && vis->m_geometry.m_hasLocalMaterial) + { + for (int i = 0; i < 4; i++) + { + rgbaColor[i] = vis->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[i]; + } + } + + } + } + + } + else + { + + if (vis && vis->m_geometry.m_hasLocalMaterial) + { + for (int i = 0; i < 4; i++) + { + rgbaColor[i] = vis->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[i]; + } + } + } + + TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + if (visualsPtr==0) + { + m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new TinyRendererObjectArray); + } + visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + + btAssert(visualsPtr); + TinyRendererObjectArray* visuals = *visualsPtr; + visuals->m_objectUniqueId = bodyUniqueId; + visuals->m_linkIndex = linkIndex; + + b3VisualShapeData visualShape; + visualShape.m_objectUniqueId = bodyUniqueId; + visualShape.m_linkIndex = linkIndex; + visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0]; + visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1]; + visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2]; + visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0]; + visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1]; + visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2]; + visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3]; + visualShape.m_rgbaColor[0] = rgbaColor[0]; + visualShape.m_rgbaColor[1] = rgbaColor[1]; + visualShape.m_rgbaColor[2] = rgbaColor[2]; + visualShape.m_rgbaColor[3] = rgbaColor[3]; + { + B3_PROFILE("convertURDFToVisualShape"); + convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures, visualShape); + } + m_data->m_visualShapes.push_back(visualShape); + + if (vertices.size() && indices.size()) + { + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); + unsigned char* textureImage1=0; + int textureWidth=0; + int textureHeight=0; + bool isCached = false; + if (textures.size()) + { + textureImage1 = textures[0].textureData1; + textureWidth = textures[0].m_width; + textureHeight = textures[0].m_height; + isCached = textures[0].m_isCached; + } + + { + B3_PROFILE("registerMeshShape"); + + tinyObj->registerMeshShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), rgbaColor, + textureImage1, textureWidth, textureHeight); + } + visuals->m_renderObjects.push_back(tinyObj); + + // register mesh to m_instancingRenderer too. + int textureIndex = m_data->m_instancingRenderer->registerTexture(textureImage1, textureWidth, textureHeight); + int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),textureIndex); + //m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0]); + + + + + } + for (int i=0;im_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId) + { + start = i; + break; + } + } + int count = 0; + + if (start >= 0) + { + for (int i = start; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId) + { + count++; + } + else + { + //storage of each visual shape for a given body unique id assumed to be contiguous + break; + } + } + } + return count; +} + +int TinyRendererVisualShapeConverter::getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData) +{ + int start = -1; + //find first one, then count how many + for (int i = 0; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId) + { + start = i; + break; + } + } + //int count = 0; + + if (start >= 0) + { + if (start + shapeIndex < m_data->m_visualShapes.size()) + { + *shapeData = m_data->m_visualShapes[start + shapeIndex]; + return 1; + } + } + return 0; +} + + + +void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]) +{ + int start = -1; + for (int i = 0; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId && m_data->m_visualShapes[i].m_linkIndex == linkIndex) + { + m_data->m_visualShapes[i].m_rgbaColor[0] = rgbaColor[0]; + m_data->m_visualShapes[i].m_rgbaColor[1] = rgbaColor[1]; + m_data->m_visualShapes[i].m_rgbaColor[2] = rgbaColor[2]; + m_data->m_visualShapes[i].m_rgbaColor[3] = rgbaColor[3]; + } + } + + for (int i=0;im_swRenderInstances.size();i++) + { + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); + if (ptrptr && *ptrptr) + { + float rgba[4] = {(float)rgbaColor[0], (float)rgbaColor[1], (float)rgbaColor[2], (float)rgbaColor[3]}; + TinyRendererObjectArray* visuals = *ptrptr; + if ((bodyUniqueId == visuals->m_objectUniqueId) && (linkIndex == visuals->m_linkIndex)) + { + for (int q=0;qm_renderObjects.size();q++) + { + if (shapeIndex<0 || q==shapeIndex) + { + visuals->m_renderObjects[q]->m_model->setColorRGBA(rgba); + } + } + } + } + } +} + + + +void TinyRendererVisualShapeConverter::setUpAxis(int axis) +{ + m_data->m_upAxis = axis; + m_data->m_camera.setCameraUpAxis(axis); + m_data->m_camera.update(); +} +void TinyRendererVisualShapeConverter::resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ) +{ + m_data->m_camera.setCameraDistance(camDist); + m_data->m_camera.setCameraPitch(pitch); + m_data->m_camera.setCameraYaw(yaw); + m_data->m_camera.setCameraTargetPosition(camPosX,camPosY,camPosZ); + m_data->m_camera.setAspectRatio((float)m_data->m_swWidth/(float)m_data->m_swHeight); + m_data->m_camera.update(); + +} + +void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor) +{ + float farPlane = m_data->m_camera.getCameraFrustumFar(); + for(int y=0;ym_swHeight;++y) + { + for(int x=0;xm_swWidth;++x) + { + m_data->m_rgbColorBuffer.set(x,y,clearColor); + m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -farPlane; + m_data->m_shadowBuffer[x+y*m_data->m_swWidth] = -1e30f; + m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1; + } + } + +} + +void TinyRendererVisualShapeConverter::render() +{ + + ATTRIBUTE_ALIGNED16(float viewMat[16]); + ATTRIBUTE_ALIGNED16(float projMat[16]); + + m_data->m_camera.getCameraProjectionMatrix(projMat); + m_data->m_camera.getCameraViewMatrix(viewMat); + + render(viewMat,projMat); + + m_data->m_instancingRenderer->renderScene(); + b3Warning("m_instancingRendere->renderScene"); +} + + +void TinyRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId, const btTransform& worldTransform, const btVector3& localScaling) +{ + TinyRendererObjectArray** renderObjPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + if (renderObjPtr) + { + TinyRendererObjectArray* renderObj = *renderObjPtr; + renderObj->m_worldTransform = worldTransform; + renderObj->m_localScaling = localScaling; + } +} + + +void TinyRendererVisualShapeConverter::render(const float viewMat[16], const float projMat[16]) +{ + //clear the color buffer + TGAColor clearColor; + clearColor.bgra[0] = 255; + clearColor.bgra[1] = 255; + clearColor.bgra[2] = 255; + clearColor.bgra[3] = 255; + + float near = projMat[14]/(projMat[10]-1); + float far = projMat[14]/(projMat[10]+1); + + m_data->m_camera.setCameraFrustumNear( near); + m_data->m_camera.setCameraFrustumFar(far); + + clearBuffers(clearColor); + + ATTRIBUTE_ALIGNED16(btScalar modelMat[16]); + + + btVector3 lightDirWorld(-5,200,-40); + if (m_data->m_hasLightDirection) + { + lightDirWorld = m_data->m_lightDirection; + } + else + { + switch (m_data->m_upAxis) + { + case 1: + lightDirWorld = btVector3(-50.f, 100, 30); + break; + case 2: + lightDirWorld = btVector3(-50.f, 30, 100); + break; + default: {} + }; + } + + lightDirWorld.normalize(); + + btVector3 lightColor(1.0,1.0,1.0); + if (m_data->m_hasLightColor) + { + lightColor = m_data->m_lightColor; + } + + float lightDistance = 2.0; + if (m_data->m_hasLightDistance) + { + lightDistance = m_data->m_lightDistance; + } + + float lightAmbientCoeff = 0.6; + if (m_data->m_hasLightAmbientCoeff) + { + lightAmbientCoeff = m_data->m_lightAmbientCoeff; + } + + float lightDiffuseCoeff = 0.35; + if (m_data->m_hasLightDiffuseCoeff) + { + lightDiffuseCoeff = m_data->m_lightDiffuseCoeff; + } + + float lightSpecularCoeff = 0.05; + if (m_data->m_hasLightSpecularCoeff) + { + lightSpecularCoeff = m_data->m_lightSpecularCoeff; + } + + if (m_data->m_hasShadow) + { + for (int n=0;nm_swRenderInstances.size();n++) + { + TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n); + if (0==visualArrayPtr) + continue;//can this ever happen? + TinyRendererObjectArray* visualArray = *visualArrayPtr; + + for (int v=0;vm_renderObjects.size();v++) + { + + TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; + + + //sync the object transform + const btTransform& tr = visualArray->m_worldTransform; + tr.getOpenGLMatrix(modelMat); + + for (int i=0;i<4;i++) + { + for (int j=0;j<4;j++) + { + + renderObj->m_projectionMatrix[i][j] = projMat[i+4*j]; + renderObj->m_modelMatrix[i][j] = modelMat[i+4*j]; + renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; + } + } + renderObj->m_localScaling = visualArray->m_localScaling; + renderObj->m_lightDirWorld = lightDirWorld; + renderObj->m_lightColor = lightColor; + renderObj->m_lightDistance = lightDistance; + renderObj->m_lightAmbientCoeff = lightAmbientCoeff; + renderObj->m_lightDiffuseCoeff = lightDiffuseCoeff; + renderObj->m_lightSpecularCoeff = lightSpecularCoeff; + TinyRenderer::renderObjectDepth(*renderObj); + } + } + } + + for (int n=0;nm_swRenderInstances.size();n++) + { + TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n); + if (0==visualArrayPtr) + continue;//can this ever happen? + TinyRendererObjectArray* visualArray = *visualArrayPtr; + + + for (int v=0;vm_renderObjects.size();v++) + { + + TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; + + + //sync the object transform + const btTransform& tr = visualArray->m_worldTransform; + tr.getOpenGLMatrix(modelMat); + + for (int i=0;i<4;i++) + { + for (int j=0;j<4;j++) + { + + renderObj->m_projectionMatrix[i][j] = projMat[i+4*j]; + renderObj->m_modelMatrix[i][j] = modelMat[i+4*j]; + renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; + } + } + renderObj->m_localScaling = visualArray->m_localScaling; + renderObj->m_lightDirWorld = lightDirWorld; + renderObj->m_lightColor = lightColor; + renderObj->m_lightDistance = lightDistance; + renderObj->m_lightAmbientCoeff = lightAmbientCoeff; + renderObj->m_lightDiffuseCoeff = lightDiffuseCoeff; + renderObj->m_lightSpecularCoeff = lightSpecularCoeff; + TinyRenderer::renderObject(*renderObj); + } + } + //printf("write tga \n"); + //m_data->m_rgbColorBuffer.write_tga_file("camera.tga"); +// printf("flipped!\n"); + m_data->m_rgbColorBuffer.flip_vertically(); + + //flip z-buffer and segmentation Buffer + { + int half = m_data->m_swHeight>>1; + for (int j=0; jm_swWidth; + unsigned long l2 = (m_data->m_swHeight-1-j)*m_data->m_swWidth; + for (int i=0;im_swWidth;i++) + { + btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]); + btSwap(m_data->m_shadowBuffer[l1+i],m_data->m_shadowBuffer[l2+i]); + btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]); + } + } + } +} + +void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height) +{ + width = m_data->m_swWidth; + 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_shadowBuffer.resize(m_data->m_swWidth*m_data->m_swHeight); + m_data->m_segmentationMaskBuffer.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* segmentationMaskBuffer, int segmentationMaskSizeInPixels, + int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) +{ + int w = m_data->m_rgbColorBuffer.get_width(); + int h = m_data->m_rgbColorBuffer.get_height(); + + if (numPixelsCopied) + *numPixelsCopied = 0; + + if (widthPtr) + *widthPtr = w; + + if (heightPtr) + *heightPtr = h; + + int numTotalPixels = w*h; + int numRemainingPixels = numTotalPixels - startPixelIndex; + int numBytesPerPixel = 4;//RGBA + int numRequestedPixels = btMin(rgbaBufferSizeInPixels,numRemainingPixels); + if (numRequestedPixels) + { + for (int i=0;im_camera.getCameraFrustumFar(); + float nearPlane = m_data->m_camera.getCameraFrustumNear(); + + // TinyRenderer returns clip coordinates, transform to eye coordinates first + float z_c = -m_data->m_depthBuffer[i+startPixelIndex]; + // float distance = (farPlane - nearPlane) / (farPlane + nearPlane) * (z_c + 2. * farPlane * nearPlane / (farPlane - nearPlane)); + + // The depth buffer value is between 0 and 1 + // float a = farPlane / (farPlane - nearPlane); + // float b = farPlane * nearPlane / (nearPlane - farPlane); + // depthBuffer[i] = a + b / distance; + + // Simply the above expressions + depthBuffer[i] = farPlane * (nearPlane + z_c) / (2. * farPlane * nearPlane + farPlane * z_c - nearPlane * z_c); + } + if (segmentationMaskBuffer) + { + int segMask = m_data->m_segmentationMaskBuffer[i + startPixelIndex]; + if ((m_data->m_flags & ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)==0) + { + //if we don't explicitly request link index, clear it out + //object index are the lower 24bits + if (segMask >= 0) + { + segMask &= ((1 << 24) - 1); + } + } + segmentationMaskBuffer[i] = segMask; + } + + if (pixelsRGBA) + { + pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0]; + pixelsRGBA[i*numBytesPerPixel+1] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+1]; + pixelsRGBA[i*numBytesPerPixel+2] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+2]; + pixelsRGBA[i*numBytesPerPixel+3] = 255; + + } + } + + if (numPixelsCopied) + *numPixelsCopied = numRequestedPixels; + + } + + // Copied from SimpleOpenGL3App::getScreenPixels + int width = m_data->m_instancingRenderer->getScreenWidth(); + int height =m_data->m_instancingRenderer->getScreenHeight(); + if ((width*height*4) == rgbaBufferSizeInPixels) + { + glReadPixels(0,0,width, height, GL_RGBA, GL_UNSIGNED_BYTE, pixelsRGBA); + int glstat; + glstat = glGetError(); + b3Assert(glstat==GL_NO_ERROR); + } + if ((width*height*sizeof(float)) == depthBufferSizeInPixels) + { + glReadPixels(0,0,width, height, GL_DEPTH_COMPONENT, GL_FLOAT, depthBuffer); + int glstat; + glstat = glGetError(); + b3Assert(glstat==GL_NO_ERROR); + } + b3Warning("instancingRenderer getScreenPixels"); + + +} + +void TinyRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqueId) +{ + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + if (ptr) + { + for (int o=0;om_renderObjects.size();o++) + { + delete ptr->m_renderObjects[o]; + } + } + delete ptr; + m_data->m_swRenderInstances.remove(collisionObjectUniqueId); + } +} + + +void TinyRendererVisualShapeConverter::resetAll() +{ + for (int i=0;im_swRenderInstances.size();i++) + { + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + if (ptr) + { + for (int o=0;om_renderObjects.size();o++) + { + delete ptr->m_renderObjects[o]; + } + } + delete ptr; + } + } + + for (int i=0;im_textures.size();i++) + { + if (!m_data->m_textures[i].m_isCached) + { + free(m_data->m_textures[i].textureData1); + } + } + m_data->m_textures.clear(); + m_data->m_swRenderInstances.clear(); + m_data->m_visualShapes.clear(); +} + + +void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) +{ + btAssert(textureUniqueId < m_data->m_textures.size()); + if (textureUniqueId >= 0 && textureUniqueId < m_data->m_textures.size()) + { + for (int n = 0; n < m_data->m_swRenderInstances.size(); n++) + { + TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n); + if (0 == visualArrayPtr) + continue;//can this ever happen? + TinyRendererObjectArray* visualArray = *visualArrayPtr; + + if (visualArray->m_objectUniqueId == objectUniqueId && visualArray->m_linkIndex == jointIndex) + { + for (int v = 0; v < visualArray->m_renderObjects.size(); v++) + { + TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; + if ((shapeIndex < 0) || (shapeIndex == v)) + { + renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height); + } + } + } + + } + } +} + +int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) +{ + MyTexture2 texData; + texData.m_width = width; + texData.m_height = height; + texData.textureData1 = texels; + texData.m_isCached = false; + m_data->m_textures.push_back(texData); + return m_data->m_textures.size()-1; +} + +int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) +{ + B3_PROFILE("loadTextureFile"); + int width,height,n; + unsigned char* image=0; + image = stbi_load(filename, &width, &height, &n, 3); + if (image && (width>=0) && (height>=0)) + { + return registerTexture(image, width, height); + } + return -1; +} diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h new file mode 100644 index 000000000..22e85e473 --- /dev/null +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h @@ -0,0 +1,63 @@ +#ifndef EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H +#define EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H + +#include "../../../Importers/ImportURDFDemo/UrdfRenderingInterface.h" + +struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface +{ + + struct TinyRendererVisualShapeConverterInternalData* m_data; + + TinyRendererVisualShapeConverter(); + + virtual ~TinyRendererVisualShapeConverter(); + + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex); + + virtual int getNumVisualShapes(int bodyUniqueId); + + virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData); + + virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]); + + virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId); + + virtual void removeVisualShape(int shapeUid); + + virtual void setUpAxis(int axis); + + virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ); + + virtual void clearBuffers(struct TGAColor& clearColor); + + virtual void resetAll(); + + virtual void getWidthAndHeight(int& width, int& height); + virtual void setWidthAndHeight(int width, int height); + virtual void setLightDirection(float x, float y, float z); + virtual void setLightColor(float x, float y, float z); + virtual void setLightDistance(float dist); + virtual void setLightAmbientCoeff(float ambientCoeff); + virtual void setLightDiffuseCoeff(float diffuseCoeff); + virtual void setLightSpecularCoeff(float specularCoeff); + virtual void setShadow(bool hasShadow); + virtual void setFlags(int flags); + + virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); + + virtual void render(); + virtual void render(const float viewMat[16], const float projMat[16]); + + virtual int loadTextureFile(const char* filename); + virtual int registerTexture(unsigned char* texels, int width, int height); + + + + virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling); + +}; + + + + +#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H diff --git a/examples/SharedMemory/plugins/eglPlugin/premake4.lua b/examples/SharedMemory/plugins/eglPlugin/premake4.lua new file mode 100644 index 000000000..fd2b91db8 --- /dev/null +++ b/examples/SharedMemory/plugins/eglPlugin/premake4.lua @@ -0,0 +1,53 @@ + + +project ("pybullet_eglRendererPlugin") + language "C++" + kind "SharedLib" + + includedirs {".","../../../../src", "../../../../examples", + "../../../ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + hasCL = findOpenCL("clew") + + links{"BulletCollision", "Bullet3Common", "LinearMath"} + + if os.is("MacOSX") then +-- targetextension {"so"} + links{"Cocoa.framework","Python"} + end + + + files { + "eglRendererPlugin.cpp", + "eglRendererPlugin.h", + "TinyRendererVisualShapeConverter.cpp", + "TinyRendererVisualShapeConverter.h", + "../../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../../Importers/ImportColladaDemo/LoadMeshFromCollada.h", + "../../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../../Importers/ImportMeshUtility/b3ImportMeshUtility.h", + "../../../Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../../Importers/ImportObjDemo/LoadMeshFromObj.h", + "../../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h", + "../../../TinyRenderer/geometry.cpp", + "../../../TinyRenderer/model.cpp", + "../../../TinyRenderer/our_gl.cpp", + "../../../TinyRenderer/tgaimage.cpp", + "../../../TinyRenderer/TinyRenderer.cpp", + "../../../ThirdPartyLibs/Wavefront/egl_obj_loader.cpp", + "../../../ThirdPartyLibs/Wavefront/egl_obj_loader.h", + "../../../ThirdPartyLibs/stb_image/stb_image.cpp", + "../../../ThirdPartyLibs/stb_image/stb_image.h", + "../../../ThirdPartyLibs/eglxml2/eglxml2.cpp", + "../../../ThirdPartyLibs/eglxml2/eglxml2.h", + "../../../OpenGLWindow/SimpleCamera.cpp", + "../../../OpenGLWindow/SimpleCamera.h", + "../../../Utils/b3Clock.cpp", + "../../../Utils/b3Clock.h", + "../../../Utils/b3ResourcePath.cpp", + "../../../Utils/b3ResourcePath.h", + } + + + diff --git a/setup.py b/setup.py index 3baaa0348..4524ad87b 100644 --- a/setup.py +++ b/setup.py @@ -394,6 +394,7 @@ if _platform == "linux" or _platform == "linux2": CXX_FLAGS += '-DDYNAMIC_LOAD_X11_FUNCTIONS ' CXX_FLAGS += '-DHAS_SOCKLEN_T ' CXX_FLAGS += '-fno-inline-functions-called-once ' + CXX_FLAGS += '-fPIC ' # for plugins sources = sources + ["examples/ThirdPartyLibs/enet/unix.c"]\ +["examples/OpenGLWindow/X11OpenGLWindow.cpp"]\ +["examples/ThirdPartyLibs/glad/gl.c"]\ @@ -438,6 +439,7 @@ else: +["examples/ThirdPartyLibs/glad/gl.c"]\ + sources + setup_py_dir = os.path.dirname(os.path.realpath(__file__)) need_files = [] @@ -458,6 +460,52 @@ print("packages") print(find_packages('examples/pybullet/gym')) print("-----") +egl_renderer_sources = \ +["examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp"]\ ++["examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp"]\ ++["examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp"]\ ++["examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp"]\ ++["examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp"]\ ++["examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp"]\ ++["examples/TinyRenderer/geometry.cpp"]\ ++["examples/TinyRenderer/model.cpp"]\ ++["examples/TinyRenderer/tgaimage.cpp"]\ ++["examples/TinyRenderer/our_gl.cpp"]\ ++["examples/TinyRenderer/TinyRenderer.cpp"]\ ++["examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp"]\ ++["examples/ThirdPartyLibs/stb_image/stb_image.cpp"]\ ++["examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp"]\ ++["examples/OpenGLWindow/SimpleCamera.cpp"]\ ++["examples/Utils/b3Clock.cpp"]\ ++["examples/Utils/b3ResourcePath.cpp"]\ ++["src/BulletCollision/CollisionShapes/btShapeHull.cpp"]\ ++["src/BulletCollision/CollisionShapes/btConvexHullShape.cpp"]\ ++["src/BulletCollision/CollisionShapes/btBoxShape.cpp"]\ ++["src/BulletCollision/CollisionShapes/btSphereShape.cpp"]\ ++["src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp"]\ ++["src/BulletCollision/CollisionShapes/btConvexShape.cpp"]\ ++["src/BulletCollision/CollisionShapes/btCollisionShape.cpp"]\ ++["src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp"]\ ++["src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp"]\ ++["src/Bullet3Common/b3Logging.cpp"]\ ++["src/LinearMath/btAlignedAllocator.cpp"]\ ++["src/LinearMath/btGeometryUtil.cpp"]\ ++["src/LinearMath/btConvexHull.cpp"]\ ++["src/LinearMath/btConvexHullComputer.cpp"]\ ++["src/Bullet3Common/b3AlignedAllocator.cpp"] \ ++["examples/ThirdPartyLibs/glad/glad.c"]\ ++["examples/ThirdPartyLibs/glad/glad_glx.c"] \ ++["examples/OpenGLWindow/GLInstancingRenderer.cpp"]\ ++["examples/OpenGLWindow/EGLOpenGLWindow.cpp"]\ ++["examples/OpenGLWindow/GLRenderToTexture.cpp"]\ + + +eglRender = Extension("eglRenderer", + sources = egl_renderer_sources, + libraries = libraries, + extra_compile_args=(CXX_FLAGS+'-DBT_USE_EGL ').split(), + include_dirs = include_dirs + ["src","examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"] + ) setup( name = 'pybullet', version='2.1.3', @@ -469,7 +517,7 @@ setup( license='zlib', platforms='any', keywords=['game development', 'virtual reality', 'physics simulation', 'robotics', 'collision detection', 'opengl'], - ext_modules = [Extension("pybullet", + ext_modules = [eglRender, Extension("pybullet", sources = sources, libraries = libraries, extra_compile_args=CXX_FLAGS.split(),