mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
989 lines
32 KiB
C++
989 lines
32 KiB
C++
/* 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 "TinyRendererVisualShapeConverter.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 <string>
|
|
#include "../Utils/b3ResourcePath.h"
|
|
#include "../TinyRenderer/TinyRenderer.h"
|
|
#include "../OpenGLWindow/SimpleCamera.h"
|
|
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include "../Importers/ImportURDFDemo/UrdfParser.h"
|
|
#include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData
|
|
#include "../TinyRenderer/model.h"
|
|
#include "../ThirdPartyLibs/stb_image/stb_image.h"
|
|
|
|
struct MyTexture2
|
|
{
|
|
unsigned char* textureData;
|
|
int m_width;
|
|
int m_height;
|
|
};
|
|
|
|
struct TinyRendererObjectArray
|
|
{
|
|
btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects;
|
|
};
|
|
|
|
#define START_WIDTH 640
|
|
#define START_HEIGHT 480
|
|
|
|
struct TinyRendererVisualShapeConverterInternalData
|
|
{
|
|
|
|
btHashMap<btHashPtr,TinyRendererObjectArray*> m_swRenderInstances;
|
|
|
|
btAlignedObjectArray<b3VisualShapeData> m_visualShapes;
|
|
|
|
int m_upAxis;
|
|
int m_swWidth;
|
|
int m_swHeight;
|
|
TGAImage m_rgbColorBuffer;
|
|
b3AlignedObjectArray<MyTexture2> m_textures;
|
|
b3AlignedObjectArray<float> m_depthBuffer;
|
|
b3AlignedObjectArray<float> m_shadowBuffer;
|
|
b3AlignedObjectArray<int> 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;
|
|
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_depthBuffer.resize(m_swWidth*m_swHeight);
|
|
m_shadowBuffer.resize(m_swWidth*m_swHeight);
|
|
m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1);
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter()
|
|
{
|
|
m_data = new TinyRendererVisualShapeConverterInternalData();
|
|
|
|
float dist = 1.5;
|
|
float pitch = -80;
|
|
float yaw = 10;
|
|
float targetPos[3]={0,0,0};
|
|
m_data->m_camera.setCameraUpAxis(m_data->m_upAxis);
|
|
resetCamera(dist,pitch,yaw,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::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 UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& 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;
|
|
visualShapeOut.m_meshAssetFileName[0] = 0;
|
|
|
|
GLInstanceGraphicsShape* glmesh = 0;
|
|
|
|
btConvexShape* convexColShape = 0;
|
|
|
|
switch (visual->m_geometry.m_type)
|
|
{
|
|
case URDF_GEOM_CYLINDER:
|
|
{
|
|
btAlignedObjectArray<btVector3> vertices;
|
|
|
|
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
|
|
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
|
|
|
|
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
|
int numSteps = 32;
|
|
for (int i = 0; i<numSteps; i++)
|
|
{
|
|
|
|
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
|
|
btScalar cylLength = visual->m_geometry.m_cylinderLength;
|
|
|
|
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
|
vertices.push_back(vert);
|
|
vert[2] = -cylLength / 2.;
|
|
vertices.push_back(vert);
|
|
}
|
|
|
|
btConvexHullShape* cylZShape = new 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];
|
|
|
|
visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
|
|
visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
|
|
visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
|
|
visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
|
|
visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
|
|
visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
|
|
visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
|
|
|
|
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_textureImage)
|
|
{
|
|
MyTexture2 texData;
|
|
texData.m_width = meshData.m_textureWidth;
|
|
texData.m_height = meshData.m_textureHeight;
|
|
texData.textureData = meshData.m_textureImage;
|
|
texturesOut.push_back(texData);
|
|
}
|
|
glmesh = meshData.m_gfxShape;
|
|
}
|
|
break;
|
|
}
|
|
case UrdfGeometry::FILE_STL:
|
|
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
|
|
break;
|
|
case UrdfGeometry::FILE_COLLADA:
|
|
{
|
|
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
|
btAlignedObjectArray<ColladaGraphicsInstance> 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<int>();
|
|
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
|
|
|
for (int i = 0; i<visualShapeInstances.size(); i++)
|
|
{
|
|
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
|
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
|
|
|
b3AlignedObjectArray<GLInstanceVertex> verts;
|
|
verts.resize(gfxShape->m_vertices->size());
|
|
|
|
int baseIndex = glmesh->m_vertices->size();
|
|
|
|
for (int i = 0; i<gfxShape->m_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; k<additionalIndices; k++)
|
|
{
|
|
glmesh->m_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; v<verts.size(); v++)
|
|
{
|
|
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
|
|
pos = worldMat*pos;
|
|
verts[v].xyzw[0] = float(pos[0]);
|
|
verts[v].xyzw[1] = float(pos[1]);
|
|
verts[v].xyzw[2] = float(pos[2]);
|
|
glmesh->m_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;i<glmesh->m_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
|
|
|
|
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<int>();
|
|
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
|
|
|
|
|
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;
|
|
pos.normalize();
|
|
vtx.normal[0] = pos.x();
|
|
vtx.normal[1] = pos.y();
|
|
vtx.normal[2] = pos.z();
|
|
vtx.uv[0] = 0.5f;
|
|
vtx.uv[1] = 0.5f;
|
|
glmesh->m_vertices->push_back(vtx);
|
|
}
|
|
|
|
btAlignedObjectArray<int> 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;
|
|
|
|
}
|
|
|
|
|
|
|
|
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
|
|
{
|
|
|
|
|
|
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
|
|
if (linkPtr)
|
|
{
|
|
|
|
const UrdfLink* link = *linkPtr;
|
|
|
|
for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
|
|
{
|
|
btAlignedObjectArray<MyTexture2> textures;
|
|
btAlignedObjectArray<GLInstanceVertex> vertices;
|
|
btAlignedObjectArray<int> indices;
|
|
btTransform startTrans; startTrans.setIdentity();
|
|
//int graphicsIndex = -1;
|
|
|
|
const UrdfVisual& vis = link->m_visualArray[v1];
|
|
btTransform childTrans = vis.m_linkLocalFrame;
|
|
btHashString matName(vis.m_materialName.c_str());
|
|
UrdfMaterial *const * matPtr = model.m_materials[matName];
|
|
|
|
float rgbaColor[4] = {1,1,1,1};
|
|
|
|
if (matPtr)
|
|
{
|
|
UrdfMaterial *const mat = *matPtr;
|
|
for (int i=0;i<4;i++)
|
|
rgbaColor[i] = mat->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);
|
|
}
|
|
|
|
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
|
|
if (visualsPtr==0)
|
|
{
|
|
m_data->m_swRenderInstances.insert(colObj,new TinyRendererObjectArray);
|
|
}
|
|
visualsPtr = m_data->m_swRenderInstances[colObj];
|
|
btAssert(visualsPtr);
|
|
TinyRendererObjectArray* visuals = *visualsPtr;
|
|
|
|
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];
|
|
|
|
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);
|
|
unsigned char* textureImage=0;
|
|
int textureWidth=0;
|
|
int textureHeight=0;
|
|
if (textures.size())
|
|
{
|
|
textureImage = textures[0].textureData;
|
|
textureWidth = textures[0].m_width;
|
|
textureHeight = textures[0].m_height;
|
|
}
|
|
|
|
tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor,
|
|
textureImage,textureWidth,textureHeight);
|
|
visuals->m_renderObjects.push_back(tinyObj);
|
|
}
|
|
for (int i=0;i<textures.size();i++)
|
|
{
|
|
free(textures[i].textureData);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)
|
|
{
|
|
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)
|
|
{
|
|
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::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 pitch, float yaw, 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)
|
|
{
|
|
for(int y=0;y<m_data->m_swHeight;++y)
|
|
{
|
|
for(int x=0;x<m_data->m_swWidth;++x)
|
|
{
|
|
m_data->m_rgbColorBuffer.set(x,y,clearColor);
|
|
m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f;
|
|
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);
|
|
}
|
|
|
|
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;
|
|
|
|
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;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;
|
|
|
|
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n);
|
|
|
|
|
|
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
|
|
|
|
for (int v=0;v<visualArray->m_renderObjects.size();v++)
|
|
{
|
|
|
|
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
|
|
|
|
|
|
//sync the object transform
|
|
const btTransform& tr = colObj->getWorldTransform();
|
|
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 = colObj->getCollisionShape()->getLocalScaling();
|
|
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;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;
|
|
|
|
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n);
|
|
|
|
|
|
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
|
|
|
|
for (int v=0;v<visualArray->m_renderObjects.size();v++)
|
|
{
|
|
|
|
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
|
|
|
|
|
|
//sync the object transform
|
|
const btTransform& tr = colObj->getWorldTransform();
|
|
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 = colObj->getCollisionShape()->getLocalScaling();
|
|
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; j<half; j++)
|
|
{
|
|
unsigned long l1 = j*m_data->m_swWidth;
|
|
unsigned long l2 = (m_data->m_swHeight-1-j)*m_data->m_swWidth;
|
|
for (int i=0;i<m_data->m_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;i<numRequestedPixels;i++)
|
|
{
|
|
if (depthBuffer)
|
|
{
|
|
float distance = -m_data->m_depthBuffer[i+startPixelIndex];
|
|
float farPlane = m_data->m_camera.getCameraFrustumFar();
|
|
float nearPlane = m_data->m_camera.getCameraFrustumNear();
|
|
|
|
btClamp(distance,nearPlane,farPlane);
|
|
|
|
// 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;
|
|
}
|
|
if (segmentationMaskBuffer)
|
|
{
|
|
segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
|
|
}
|
|
|
|
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;
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void TinyRendererVisualShapeConverter::resetAll()
|
|
{
|
|
for (int i=0;i<m_data->m_swRenderInstances.size();i++)
|
|
{
|
|
TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i);
|
|
if (ptrptr && *ptrptr)
|
|
{
|
|
TinyRendererObjectArray* ptr = *ptrptr;
|
|
if (ptr)
|
|
{
|
|
for (int o=0;o<ptr->m_renderObjects.size();o++)
|
|
{
|
|
delete ptr->m_renderObjects[o];
|
|
}
|
|
}
|
|
delete ptr;
|
|
}
|
|
}
|
|
|
|
m_data->m_swRenderInstances.clear();
|
|
}
|
|
|
|
void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId)
|
|
{
|
|
btAssert(textureUniqueId < m_data->m_textures.size());
|
|
TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId);
|
|
if (ptrptr && *ptrptr)
|
|
{
|
|
TinyRendererObjectArray* ptr = *ptrptr;
|
|
ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height);
|
|
}
|
|
}
|
|
|
|
void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
|
|
{
|
|
int start = -1;
|
|
for (int i = 0; i < m_data->m_visualShapes.size(); i++)
|
|
{
|
|
if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex)
|
|
{
|
|
start = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (start >= 0)
|
|
{
|
|
if (start + shapeIndex < m_data->m_visualShapes.size())
|
|
{
|
|
activateShapeTexture(start + shapeIndex, textureUniqueId);
|
|
}
|
|
}
|
|
}
|
|
|
|
int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height)
|
|
{
|
|
MyTexture2 texData;
|
|
texData.m_width = width;
|
|
texData.m_height = height;
|
|
texData.textureData = texels;
|
|
m_data->m_textures.push_back(texData);
|
|
return m_data->m_textures.size()-1;
|
|
}
|
|
|
|
int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
|
|
{
|
|
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;
|
|
}
|