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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

179
examples/Importers/ImportBsp/BspConverter.cpp
View File

@@ -20,60 +20,55 @@ subject to the following restrictions:
#include <stdio.h>
#include <string.h>
void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
void BspConverter::convertBsp(BspLoader& bspLoader, float scaling)
{
{
float playstartf[3] = {0,0,100};
float playstartf[3] = {0, 0, 100};
if (bspLoader.findVectorByName(&playstartf[0],"info_player_start"))
if (bspLoader.findVectorByName(&playstartf[0], "info_player_start"))
{
printf("found playerstart\n");
}
}
else
{
if (bspLoader.findVectorByName(&playstartf[0],"info_player_deathmatch"))
if (bspLoader.findVectorByName(&playstartf[0], "info_player_deathmatch"))
{
printf("found deatchmatch start\n");
}
}
btVector3 playerStart (playstartf[0],playstartf[1],playstartf[2]);
btVector3 playerStart(playstartf[0], playstartf[1], playstartf[2]);
playerStart[2] += 20.f; //start a bit higher
playerStart[2] += 20.f; //start a bit higher
playerStart *= scaling;
//progressBegin("Loading bsp");
for (int i=0;i<bspLoader.m_numleafs;i++)
for (int i = 0; i < bspLoader.m_numleafs; i++)
{
printf("Reading bspLeaf %i from total %i (%f procent)\n",i, bspLoader.m_numleafs,(100.f*(float)i/float(bspLoader.m_numleafs)) );
printf("Reading bspLeaf %i from total %i (%f procent)\n", i, bspLoader.m_numleafs, (100.f * (float)i / float(bspLoader.m_numleafs)));
bool isValidBrush = false;
BSPLeaf& leaf = bspLoader.m_dleafs[i];
for (int b=0;b<leaf.numLeafBrushes;b++)
BSPLeaf& leaf = bspLoader.m_dleafs[i];
for (int b = 0; b < leaf.numLeafBrushes; b++)
{
btAlignedObjectArray<btVector3> planeEquations;
int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush+b];
int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush + b];
BSPBrush& brush = bspLoader.m_dbrushes[brushid];
if (brush.shaderNum!=-1)
if (brush.shaderNum != -1)
{
if (bspLoader.m_dshaders[ brush.shaderNum ].contentFlags & BSPCONTENTS_SOLID)
if (bspLoader.m_dshaders[brush.shaderNum].contentFlags & BSPCONTENTS_SOLID)
{
brush.shaderNum = -1;
for (int p=0;p<brush.numSides;p++)
for (int p = 0; p < brush.numSides; p++)
{
int sideid = brush.firstSide+p;
int sideid = brush.firstSide + p;
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
@@ -82,45 +77,44 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
plane.normal[0],
plane.normal[1],
plane.normal[2]);
planeEq[3] = scaling*-plane.dist;
planeEq[3] = scaling * -plane.dist;
planeEquations.push_back(planeEq);
isValidBrush=true;
isValidBrush = true;
}
if (isValidBrush)
{
btAlignedObjectArray<btVector3> vertices;
btGeometryUtil::getVerticesFromPlaneEquations(planeEquations, vertices);
btAlignedObjectArray<btVector3> vertices;
btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices);
bool isEntity = false;
btVector3 entityTarget(0.f,0.f,0.f);
addConvexVerticesCollider(vertices,isEntity,entityTarget);
btVector3 entityTarget(0.f, 0.f, 0.f);
addConvexVerticesCollider(vertices, isEntity, entityTarget);
}
}
}
}
}
}
#define USE_ENTITIES
#ifdef USE_ENTITIES
{
int i;
for (i=0;i<bspLoader.m_num_entities;i++)
for (i = 0; i < bspLoader.m_num_entities; i++)
{
const BSPEntity& entity = bspLoader.m_entities[i];
const char* cl = bspLoader.getValueForKey(&entity, "classname");
if (!strcmp(cl, "trigger_push"))
{
const BSPEntity& entity = bspLoader.m_entities[i];
const char* cl = bspLoader.getValueForKey(&entity,"classname");
if ( !strcmp( cl, "trigger_push" ) ) {
btVector3 targetLocation(0.f,0.f,0.f);
btVector3 targetLocation(0.f, 0.f, 0.f);
cl = bspLoader.getValueForKey(&entity,"target");
if ( strcmp( cl, "" ) ) {
//its not empty so ...
cl = bspLoader.getValueForKey(&entity, "target");
if (strcmp(cl, ""))
{
//its not empty so ...
/*
/*
//lookup the target position for the jumppad:
const BSPEntity* targetentity = bspLoader.getEntityByValue( "targetname" , cl );
if (targetentity)
@@ -132,76 +126,63 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
}
*/
cl = bspLoader.getValueForKey(&entity,"model");
if ( strcmp( cl, "" ) ) {
// add the model as a brush
if (cl[0] == '*')
cl = bspLoader.getValueForKey(&entity, "model");
if (strcmp(cl, ""))
{
// add the model as a brush
if (cl[0] == '*')
{
int modelnr = atoi(&cl[1]);
if ((modelnr >= 0) && (modelnr < bspLoader.m_nummodels))
{
int modelnr = atoi(&cl[1]);
if ((modelnr >=0) && (modelnr < bspLoader.m_nummodels))
const BSPModel& model = bspLoader.m_dmodels[modelnr];
for (int n = 0; n < model.numBrushes; n++)
{
const BSPModel& model = bspLoader.m_dmodels[modelnr];
for (int n=0;n<model.numBrushes;n++)
btAlignedObjectArray<btVector3> planeEquations;
bool isValidBrush = false;
//convert brush
const BSPBrush& brush = bspLoader.m_dbrushes[model.firstBrush + n];
{
btAlignedObjectArray<btVector3> planeEquations;
bool isValidBrush = false;
//convert brush
const BSPBrush& brush = bspLoader.m_dbrushes[model.firstBrush+n];
for (int p = 0; p < brush.numSides; p++)
{
for (int p=0;p<brush.numSides;p++)
{
int sideid = brush.firstSide+p;
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
btVector3 planeEq;
planeEq.setValue(
plane.normal[0],
plane.normal[1],
plane.normal[2]);
planeEq[3] = scaling*-plane.dist;
planeEquations.push_back(planeEq);
isValidBrush=true;
}
if (isValidBrush)
{
btAlignedObjectArray<btVector3> vertices;
btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices);
bool isEntity=true;
addConvexVerticesCollider(vertices,isEntity,targetLocation);
}
int sideid = brush.firstSide + p;
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
btVector3 planeEq;
planeEq.setValue(
plane.normal[0],
plane.normal[1],
plane.normal[2]);
planeEq[3] = scaling * -plane.dist;
planeEquations.push_back(planeEq);
isValidBrush = true;
}
if (isValidBrush)
{
btAlignedObjectArray<btVector3> vertices;
btGeometryUtil::getVerticesFromPlaneEquations(planeEquations, vertices);
bool isEntity = true;
addConvexVerticesCollider(vertices, isEntity, targetLocation);
}
}
}
}
else
{
printf("unsupported trigger_push model, md3 ?\n");
}
}
else
{
printf("unsupported trigger_push model, md3 ?\n");
}
}
}
}
}
#endif //USE_ENTITIES
}
#endif //USE_ENTITIES
//progressEnd();
}
}
}

19
examples/Importers/ImportBsp/BspConverter.h
View File

@@ -23,17 +23,14 @@ class BspLoader;
///BspConverter turns a loaded bsp level into convex parts (vertices)
class BspConverter
{
public:
void convertBsp(BspLoader& bspLoader,float scaling);
virtual ~BspConverter()
{
}
///this callback is called for each brush that succesfully converted into vertices
virtual void addConvexVerticesCollider(btAlignedObjectArray<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation) = 0;
public:
void convertBsp(BspLoader& bspLoader, float scaling);
virtual ~BspConverter()
{
}
///this callback is called for each brush that succesfully converted into vertices
virtual void addConvexVerticesCollider(btAlignedObjectArray<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation) = 0;
};
#endif //BSP_CONVERTER_H
#endif //BSP_CONVERTER_H

507
examples/Importers/ImportBsp/BspLoader.cpp
View File

@@ -20,26 +20,25 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
===========================================================================
*/
#include "BspLoader.h"
#include <stdio.h>
#include <string.h>
typedef struct
{
char filename[1024];
char *buffer,*script_p,*end_p;
int line;
char filename[1024];
char *buffer, *script_p, *end_p;
int line;
} BSPScript;
#define MAX_INCLUDES 8
BSPScript scriptstack[MAX_INCLUDES];
BSPScript *script;
int scriptline;
#define MAX_INCLUDES 8
BSPScript scriptstack[MAX_INCLUDES];
BSPScript *script;
int scriptline;
char token[BSPMAXTOKEN];
char token[BSPMAXTOKEN];
bool endofscript;
bool tokenready; // only true if UnGetToken was just called
bool tokenready; // only true if UnGetToken was just called
//
//loadBSPFile
@@ -48,143 +47,137 @@ bool tokenready; // only true if UnGetToken was just called
int extrasize = 100;
BspLoader::BspLoader()
:m_num_entities(0)
: m_num_entities(0)
{
m_Endianness = getMachineEndianness();
if (m_Endianness == BSP_BIG_ENDIAN)
{
printf("Machine is BIG_ENDIAN\n");
} else
}
else
{
printf("Machine is Little Endian\n");
}
}
bool BspLoader::loadBSPFile( void* memoryBuffer) {
BSPHeader *header = (BSPHeader*) memoryBuffer;
bool BspLoader::loadBSPFile(void *memoryBuffer)
{
BSPHeader *header = (BSPHeader *)memoryBuffer;
// load the file header
if (header)
{
// swap the header
swapBlock( (int *)header, sizeof(*header) );
swapBlock((int *)header, sizeof(*header));
int length = (header->lumps[BSPLUMP_SHADERS].filelen) / sizeof(BSPShader);
m_dshaders.resize(length+extrasize);
m_numShaders = copyLump( header, BSPLUMP_SHADERS, &m_dshaders[0], sizeof(BSPShader) );
m_dshaders.resize(length + extrasize);
m_numShaders = copyLump(header, BSPLUMP_SHADERS, &m_dshaders[0], sizeof(BSPShader));
length = (header->lumps[LUMP_MODELS].filelen) / sizeof(BSPModel);
m_dmodels.resize(length+extrasize);
m_nummodels = copyLump( header, LUMP_MODELS, &m_dmodels[0], sizeof(BSPModel) );
m_dmodels.resize(length + extrasize);
m_nummodels = copyLump(header, LUMP_MODELS, &m_dmodels[0], sizeof(BSPModel));
length = (header->lumps[BSPLUMP_PLANES].filelen) / sizeof(BSPPlane);
m_dplanes.resize(length+extrasize);
m_numplanes = copyLump( header, BSPLUMP_PLANES, &m_dplanes[0], sizeof(BSPPlane) );
m_dplanes.resize(length + extrasize);
m_numplanes = copyLump(header, BSPLUMP_PLANES, &m_dplanes[0], sizeof(BSPPlane));
length = (header->lumps[BSPLUMP_LEAFS].filelen) / sizeof(BSPLeaf);
m_dleafs.resize(length+extrasize);
m_numleafs = copyLump( header, BSPLUMP_LEAFS, &m_dleafs[0], sizeof(BSPLeaf) );
m_dleafs.resize(length + extrasize);
m_numleafs = copyLump(header, BSPLUMP_LEAFS, &m_dleafs[0], sizeof(BSPLeaf));
length = (header->lumps[BSPLUMP_NODES].filelen) / sizeof(BSPNode);
m_dnodes.resize(length+extrasize);
m_numnodes = copyLump( header, BSPLUMP_NODES, &m_dnodes[0], sizeof(BSPNode) );
m_dnodes.resize(length + extrasize);
m_numnodes = copyLump(header, BSPLUMP_NODES, &m_dnodes[0], sizeof(BSPNode));
length = (header->lumps[BSPLUMP_LEAFSURFACES].filelen) / sizeof(m_dleafsurfaces[0]);
m_dleafsurfaces.resize(length+extrasize);
m_numleafsurfaces = copyLump( header, BSPLUMP_LEAFSURFACES, &m_dleafsurfaces[0], sizeof(m_dleafsurfaces[0]) );
m_dleafsurfaces.resize(length + extrasize);
m_numleafsurfaces = copyLump(header, BSPLUMP_LEAFSURFACES, &m_dleafsurfaces[0], sizeof(m_dleafsurfaces[0]));
length = (header->lumps[BSPLUMP_LEAFBRUSHES].filelen) / sizeof(m_dleafbrushes[0]) ;
m_dleafbrushes.resize(length+extrasize);
m_numleafbrushes = copyLump( header, BSPLUMP_LEAFBRUSHES, &m_dleafbrushes[0], sizeof(m_dleafbrushes[0]) );
length = (header->lumps[BSPLUMP_LEAFBRUSHES].filelen) / sizeof(m_dleafbrushes[0]);
m_dleafbrushes.resize(length + extrasize);
m_numleafbrushes = copyLump(header, BSPLUMP_LEAFBRUSHES, &m_dleafbrushes[0], sizeof(m_dleafbrushes[0]));
length = (header->lumps[LUMP_BRUSHES].filelen) / sizeof(BSPBrush);
m_dbrushes.resize(length+extrasize);
m_numbrushes = copyLump( header, LUMP_BRUSHES, &m_dbrushes[0], sizeof(BSPBrush) );
m_dbrushes.resize(length + extrasize);
m_numbrushes = copyLump(header, LUMP_BRUSHES, &m_dbrushes[0], sizeof(BSPBrush));
length = (header->lumps[LUMP_BRUSHSIDES].filelen) / sizeof(BSPBrushSide);
m_dbrushsides.resize(length+extrasize);
m_numbrushsides = copyLump( header, LUMP_BRUSHSIDES, &m_dbrushsides[0], sizeof(BSPBrushSide) );
m_dbrushsides.resize(length + extrasize);
m_numbrushsides = copyLump(header, LUMP_BRUSHSIDES, &m_dbrushsides[0], sizeof(BSPBrushSide));
length = (header->lumps[LUMP_SURFACES].filelen) / sizeof(BSPSurface);
m_drawSurfaces.resize(length+extrasize);
m_numDrawSurfaces = copyLump( header, LUMP_SURFACES, &m_drawSurfaces[0], sizeof(BSPSurface) );
m_drawSurfaces.resize(length + extrasize);
m_numDrawSurfaces = copyLump(header, LUMP_SURFACES, &m_drawSurfaces[0], sizeof(BSPSurface));
length = (header->lumps[LUMP_DRAWINDEXES].filelen) / sizeof(m_drawIndexes[0]);
m_drawIndexes.resize(length+extrasize);
m_numDrawIndexes = copyLump( header, LUMP_DRAWINDEXES, &m_drawIndexes[0], sizeof(m_drawIndexes[0]) );
m_drawIndexes.resize(length + extrasize);
m_numDrawIndexes = copyLump(header, LUMP_DRAWINDEXES, &m_drawIndexes[0], sizeof(m_drawIndexes[0]));
length = (header->lumps[LUMP_VISIBILITY].filelen) / 1;
m_visBytes.resize(length+extrasize);
m_numVisBytes = copyLump( header, LUMP_VISIBILITY, &m_visBytes[0], 1 );
m_visBytes.resize(length + extrasize);
m_numVisBytes = copyLump(header, LUMP_VISIBILITY, &m_visBytes[0], 1);
length = (header->lumps[LUMP_LIGHTMAPS].filelen) / 1;
m_lightBytes.resize(length+extrasize);
m_numLightBytes = copyLump( header, LUMP_LIGHTMAPS, &m_lightBytes[0], 1 );
m_lightBytes.resize(length + extrasize);
m_numLightBytes = copyLump(header, LUMP_LIGHTMAPS, &m_lightBytes[0], 1);
length = (header->lumps[BSPLUMP_ENTITIES].filelen) / 1;
m_dentdata.resize(length+extrasize);
m_entdatasize = copyLump( header, BSPLUMP_ENTITIES, &m_dentdata[0], 1);
m_dentdata.resize(length + extrasize);
m_entdatasize = copyLump(header, BSPLUMP_ENTITIES, &m_dentdata[0], 1);
length = (header->lumps[LUMP_LIGHTGRID].filelen) / 1;
m_gridData.resize(length+extrasize);
m_numGridPoints = copyLump( header, LUMP_LIGHTGRID, &m_gridData[0], 8 );
m_gridData.resize(length + extrasize);
m_numGridPoints = copyLump(header, LUMP_LIGHTGRID, &m_gridData[0], 8);
// swap everything
swapBSPFile();
return true;
}
return false;
}
const char *BspLoader::getValueForKey(const BSPEntity *ent, const char *key) const
{
const BSPKeyValuePair *ep;
const char* BspLoader::getValueForKey( const BSPEntity* ent, const char* key ) const {
const BSPKeyValuePair* ep;
for (ep=ent->epairs ; ep ; ep=ep->next) {
if (!strcmp(ep->key, key) ) {
for (ep = ent->epairs; ep; ep = ep->next)
{
if (!strcmp(ep->key, key))
{
return ep->value;
}
}
return "";
}
float BspLoader::getFloatForKey( const BSPEntity *ent, const char *key ) {
const char *k;
k = getValueForKey( ent, key );
float BspLoader::getFloatForKey(const BSPEntity *ent, const char *key)
{
const char *k;
k = getValueForKey(ent, key);
return float(atof(k));
}
bool BspLoader::getVectorForKey( const BSPEntity *ent, const char *key, BSPVector3 vec ) {
const char *k;
k = getValueForKey (ent, key);
bool BspLoader::getVectorForKey(const BSPEntity *ent, const char *key, BSPVector3 vec)
{
const char *k;
k = getValueForKey(ent, key);
if (strcmp(k, ""))
{
sscanf (k, "%f %f %f", &vec[0], &vec[1], &vec[2]);
sscanf(k, "%f %f %f", &vec[0], &vec[1], &vec[2]);
return true;
}
return false;
}
/*
==============
parseFromMemory
==============
*/
void BspLoader::parseFromMemory (char *buffer, int size)
void BspLoader::parseFromMemory(char *buffer, int size)
{
script = scriptstack;
script++;
@@ -192,7 +185,7 @@ void BspLoader::parseFromMemory (char *buffer, int size)
{
//printf("script file exceeded MAX_INCLUDES");
}
strcpy (script->filename, "memory buffer" );
strcpy(script->filename, "memory buffer");
script->buffer = buffer;
script->line = 1;
@@ -203,20 +196,19 @@ void BspLoader::parseFromMemory (char *buffer, int size)
tokenready = false;
}
bool BspLoader::isEndOfScript (bool crossline)
bool BspLoader::isEndOfScript(bool crossline)
{
if (!crossline)
//printf("Line %i is incomplete\n",scriptline);
if (!strcmp (script->filename, "memory buffer"))
{
endofscript = true;
return false;
}
if (!strcmp(script->filename, "memory buffer"))
{
endofscript = true;
return false;
}
//free (script->buffer);
if (script == scriptstack+1)
if (script == scriptstack + 1)
{
endofscript = true;
return false;
@@ -224,7 +216,7 @@ bool BspLoader::isEndOfScript (bool crossline)
script--;
scriptline = script->line;
//printf ("returning to %s\n", script->filename);
return getToken (crossline);
return getToken(crossline);
}
/*
@@ -233,18 +225,18 @@ bool BspLoader::isEndOfScript (bool crossline)
getToken
==============
*/
bool BspLoader::getToken (bool crossline)
bool BspLoader::getToken(bool crossline)
{
char *token_p;
char *token_p;
if (tokenready) // is a token allready waiting?
if (tokenready) // is a token allready waiting?
{
tokenready = false;
return true;
}
if (script->script_p >= script->end_p)
return isEndOfScript (crossline);
return isEndOfScript(crossline);
//
// skip space
@@ -253,7 +245,7 @@ skipspace:
while (*script->script_p <= 32)
{
if (script->script_p >= script->end_p)
return isEndOfScript (crossline);
return isEndOfScript(crossline);
if (*script->script_p++ == '\n')
{
if (!crossline)
@@ -265,11 +257,10 @@ skipspace:
}
if (script->script_p >= script->end_p)
return isEndOfScript (crossline);
return isEndOfScript(crossline);
// ; # // comments
if (*script->script_p == ';' || *script->script_p == '#'
|| ( script->script_p[0] == '/' && script->script_p[1] == '/') )
if (*script->script_p == ';' || *script->script_p == '#' || (script->script_p[0] == '/' && script->script_p[1] == '/'))
{
if (!crossline)
{
@@ -277,7 +268,7 @@ skipspace:
}
while (*script->script_p++ != '\n')
if (script->script_p >= script->end_p)
return isEndOfScript (crossline);
return isEndOfScript(crossline);
scriptline = script->line++;
goto skipspace;
}
@@ -289,23 +280,24 @@ skipspace:
{
//printf("Line %i is incomplete\n",scriptline);
}
script->script_p+=2;
script->script_p += 2;
while (script->script_p[0] != '*' && script->script_p[1] != '/')
{
if ( *script->script_p == '\n' ) {
if (*script->script_p == '\n')
{
scriptline = script->line++;
}
script->script_p++;
if (script->script_p >= script->end_p)
return isEndOfScript (crossline);
return isEndOfScript(crossline);
}
script->script_p += 2;
goto skipspace;
}
//
// copy token
//
//
// copy token
//
token_p = token;
if (*script->script_p == '"')
@@ -324,25 +316,25 @@ skipspace:
}
script->script_p++;
}
else // regular token
while ( *script->script_p > 32 && *script->script_p != ';')
{
*token_p++ = *script->script_p++;
if (script->script_p == script->end_p)
break;
if (token_p == &token[BSPMAXTOKEN])
else // regular token
while (*script->script_p > 32 && *script->script_p != ';')
{
//printf ("Token too large on line %i\n",scriptline);
*token_p++ = *script->script_p++;
if (script->script_p == script->end_p)
break;
if (token_p == &token[BSPMAXTOKEN])
{
//printf ("Token too large on line %i\n",scriptline);
}
}
}
*token_p = 0;
if (!strcmp (token, "$include"))
if (!strcmp(token, "$include"))
{
//getToken (false);
//AddScriptToStack (token);
return false;//getToken (crossline);
return false; //getToken (crossline);
}
return true;
@@ -350,16 +342,17 @@ skipspace:
char *BspLoader::copystring(const char *s)
{
char *b;
b = (char*) malloc( strlen(s)+1);
strcpy (b, s);
char *b;
b = (char *)malloc(strlen(s) + 1);
strcpy(b, s);
return b;
}
void BspLoader::stripTrailing( char *e ) {
char *s;
void BspLoader::stripTrailing(char *e)
{
char *s;
s = e + strlen(e)-1;
s = e + strlen(e) - 1;
while (s >= e && *s <= 32)
{
*s = 0;
@@ -371,47 +364,50 @@ void BspLoader::stripTrailing( char *e ) {
parseEpair
=================
*/
BSPKeyValuePair *BspLoader::parseEpair( void ) {
BSPKeyValuePair *e;
BSPKeyValuePair *BspLoader::parseEpair(void)
{
BSPKeyValuePair *e;
e = (struct BSPPair*) malloc( sizeof(BSPKeyValuePair));
memset( e, 0, sizeof(BSPKeyValuePair) );
if ( strlen(token) >= BSPMAX_KEY-1 ) {
e = (struct BSPPair *)malloc(sizeof(BSPKeyValuePair));
memset(e, 0, sizeof(BSPKeyValuePair));
if (strlen(token) >= BSPMAX_KEY - 1)
{
//printf ("ParseEpar: token too long");
}
e->key = copystring( token );
getToken( false );
if ( strlen(token) >= BSPMAX_VALUE-1 ) {
e->key = copystring(token);
getToken(false);
if (strlen(token) >= BSPMAX_VALUE - 1)
{
//printf ("ParseEpar: token too long");
}
e->value = copystring( token );
e->value = copystring(token);
// strip trailing spaces that sometimes get accidentally
// added in the editor
stripTrailing( e->key );
stripTrailing( e->value );
stripTrailing(e->key);
stripTrailing(e->value);
return e;
}
/*
================
parseEntity
================
*/
bool BspLoader::parseEntity( void ) {
BSPKeyValuePair *e;
BSPEntity *mapent;
bool BspLoader::parseEntity(void)
{
BSPKeyValuePair *e;
BSPEntity *mapent;
if ( !getToken (true) ) {
if (!getToken(true))
{
return false;
}
if ( strcmp (token, "{") ) {
if (strcmp(token, "{"))
{
//printf ("parseEntity: { not found");
}
@@ -425,21 +421,24 @@ bool BspLoader::parseEntity( void ) {
bla.patches = 0;
m_entities.push_back(bla);
mapent = &m_entities[m_entities.size()-1];
mapent = &m_entities[m_entities.size() - 1];
m_num_entities++;
do {
if ( !getToken (true) ) {
do
{
if (!getToken(true))
{
//printf("parseEntity: EOF without closing brace");
}
if ( !strcmp (token, "}") ) {
if (!strcmp(token, "}"))
{
break;
}
e = (struct BSPPair*)parseEpair ();
e = (struct BSPPair *)parseEpair();
e->next = mapent->epairs;
mapent->epairs = e;
} while (1);
return true;
}
@@ -450,113 +449,108 @@ parseEntities
Parses the dentdata string into entities
================
*/
void BspLoader::parseEntities( void ) {
void BspLoader::parseEntities(void)
{
m_num_entities = 0;
m_entities.clear();
parseFromMemory( &m_dentdata[0], m_entdatasize );
parseFromMemory(&m_dentdata[0], m_entdatasize);
while ( parseEntity () ) {
}
while (parseEntity())
{
}
}
int BspLoader::getMachineEndianness()
{
long int i = 1;
const char *p = (const char *) &i;
if (p[0] == 1) // Lowest address contains the least significant byte
return BSP_LITTLE_ENDIAN;
else
return BSP_BIG_ENDIAN;
long int i = 1;
const char *p = (const char *)&i;
if (p[0] == 1) // Lowest address contains the least significant byte
return BSP_LITTLE_ENDIAN;
else
return BSP_BIG_ENDIAN;
}
short BspLoader::isLittleShort (short l)
short BspLoader::isLittleShort(short l)
{
if (machineEndianness() == BSP_BIG_ENDIAN)
{
unsigned char b1,b2;
unsigned char b1, b2;
b1 = l&255;
b2 = (l>>8)&255;
b1 = l & 255;
b2 = (l >> 8) & 255;
return (b1<<8) + b2;
return (b1 << 8) + b2;
}
//little endian
return l;
}
short BspLoader::isBigShort (short l)
short BspLoader::isBigShort(short l)
{
if (machineEndianness() == BSP_BIG_ENDIAN)
{
return l;
}
unsigned char b1,b2;
b1 = l&255;
b2 = (l>>8)&255;
return (b1<<8) + b2;
unsigned char b1, b2;
b1 = l & 255;
b2 = (l >> 8) & 255;
return (b1 << 8) + b2;
}
int BspLoader::isLittleLong (int l)
int BspLoader::isLittleLong(int l)
{
if (machineEndianness() == BSP_BIG_ENDIAN)
{
unsigned char b1,b2,b3,b4;
unsigned char b1, b2, b3, b4;
b1 = l&255;
b2 = (l>>8)&255;
b3 = (l>>16)&255;
b4 = (l>>24)&255;
b1 = l & 255;
b2 = (l >> 8) & 255;
b3 = (l >> 16) & 255;
b4 = (l >> 24) & 255;
return ((int)b1<<24) + ((int)b2<<16) + ((int)b3<<8) + b4;
return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
}
//little endian
return l;
}
int BspLoader::isBigLong (int l)
int BspLoader::isBigLong(int l)
{
if (machineEndianness() == BSP_BIG_ENDIAN)
{
return l;
}
unsigned char b1, b2, b3, b4;
unsigned char b1,b2,b3,b4;
b1 = l&255;
b2 = (l>>8)&255;
b3 = (l>>16)&255;
b4 = (l>>24)&255;
return ((int)b1<<24) + ((int)b2<<16) + ((int)b3<<8) + b4;
b1 = l & 255;
b2 = (l >> 8) & 255;
b3 = (l >> 16) & 255;
b4 = (l >> 24) & 255;
return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
}
float BspLoader::isLittleFloat (float l)
float BspLoader::isLittleFloat(float l)
{
if (machineEndianness() == BSP_BIG_ENDIAN)
{
union {unsigned char b[4]; float f;} in, out;
union {
unsigned char b[4];
float f;
} in, out;
in.f = l;
out.b[0] = in.b[3];
out.b[1] = in.b[2];
out.b[2] = in.b[1];
out.b[3] = in.b[0];
return out.f;
}
@@ -564,40 +558,40 @@ float BspLoader::isLittleFloat (float l)
return l;
}
float BspLoader::isBigFloat (float l)
float BspLoader::isBigFloat(float l)
{
if (machineEndianness() == BSP_BIG_ENDIAN)
{
return l;
}
//little endian
union {unsigned char b[4]; float f;} in, out;
union {
unsigned char b[4];
float f;
} in, out;
in.f = l;
out.b[0] = in.b[3];
out.b[1] = in.b[2];
out.b[2] = in.b[1];
out.b[3] = in.b[0];
return out.f;
}
//
// swapBlock
// If all values are 32 bits, this can be used to swap everything
//
void BspLoader::swapBlock( int *block, int sizeOfBlock ) {
int i;
void BspLoader::swapBlock(int *block, int sizeOfBlock)
{
int i;
sizeOfBlock >>= 2;
for ( i = 0 ; i < sizeOfBlock ; i++ ) {
block[i] = isLittleLong( block[i] );
for (i = 0; i < sizeOfBlock; i++)
{
block[i] = isLittleLong(block[i]);
}
}
@@ -605,99 +599,96 @@ void BspLoader::swapBlock( int *block, int sizeOfBlock ) {
// copyLump
//
int BspLoader::copyLump( BSPHeader *header, int lump, void *dest, int size ) {
int length, ofs;
int BspLoader::copyLump(BSPHeader *header, int lump, void *dest, int size)
{
int length, ofs;
length = header->lumps[lump].filelen;
ofs = header->lumps[lump].fileofs;
//if ( length % size ) {
// printf ("loadBSPFile: odd lump size");
//}
memcpy( dest, (unsigned char *)header + ofs, length );
memcpy(dest, (unsigned char *)header + ofs, length);
return length / size;
}
//
// swapBSPFile
//
void BspLoader::swapBSPFile( void ) {
int i;
// models
swapBlock( (int *) &m_dmodels[0], m_nummodels * sizeof( m_dmodels[0] ) );
void BspLoader::swapBSPFile(void)
{
int i;
// models
swapBlock((int *)&m_dmodels[0], m_nummodels * sizeof(m_dmodels[0]));
// shaders (don't swap the name)
for ( i = 0 ; i < m_numShaders ; i++ ) {
m_dshaders[i].contentFlags = isLittleLong( m_dshaders[i].contentFlags );
m_dshaders[i].surfaceFlags = isLittleLong( m_dshaders[i].surfaceFlags );
for (i = 0; i < m_numShaders; i++)
{
m_dshaders[i].contentFlags = isLittleLong(m_dshaders[i].contentFlags);
m_dshaders[i].surfaceFlags = isLittleLong(m_dshaders[i].surfaceFlags);
}
// planes
swapBlock( (int *)&m_dplanes[0], m_numplanes * sizeof( m_dplanes[0] ) );
swapBlock((int *)&m_dplanes[0], m_numplanes * sizeof(m_dplanes[0]));
// nodes
swapBlock( (int *)&m_dnodes[0], m_numnodes * sizeof( m_dnodes[0] ) );
swapBlock((int *)&m_dnodes[0], m_numnodes * sizeof(m_dnodes[0]));
// leafs
swapBlock( (int *)&m_dleafs[0], m_numleafs * sizeof( m_dleafs[0] ) );
swapBlock((int *)&m_dleafs[0], m_numleafs * sizeof(m_dleafs[0]));
// leaffaces
swapBlock( (int *)&m_dleafsurfaces[0], m_numleafsurfaces * sizeof( m_dleafsurfaces[0] ) );
swapBlock((int *)&m_dleafsurfaces[0], m_numleafsurfaces * sizeof(m_dleafsurfaces[0]));
// leafbrushes
swapBlock( (int *)&m_dleafbrushes[0], m_numleafbrushes * sizeof( m_dleafbrushes[0] ) );
swapBlock((int *)&m_dleafbrushes[0], m_numleafbrushes * sizeof(m_dleafbrushes[0]));
// brushes
swapBlock( (int *)&m_dbrushes[0], m_numbrushes * sizeof( m_dbrushes[0] ) );
swapBlock((int *)&m_dbrushes[0], m_numbrushes * sizeof(m_dbrushes[0]));
// brushsides
swapBlock( (int *)&m_dbrushsides[0], m_numbrushsides * sizeof( m_dbrushsides[0] ) );
swapBlock((int *)&m_dbrushsides[0], m_numbrushsides * sizeof(m_dbrushsides[0]));
// vis
((int *)&m_visBytes)[0] = isLittleLong( ((int *)&m_visBytes)[0] );
((int *)&m_visBytes)[1] = isLittleLong( ((int *)&m_visBytes)[1] );
((int *)&m_visBytes)[0] = isLittleLong(((int *)&m_visBytes)[0]);
((int *)&m_visBytes)[1] = isLittleLong(((int *)&m_visBytes)[1]);
// drawindexes
swapBlock( (int *)&m_drawIndexes[0], m_numDrawIndexes * sizeof( m_drawIndexes[0] ) );
swapBlock((int *)&m_drawIndexes[0], m_numDrawIndexes * sizeof(m_drawIndexes[0]));
// drawsurfs
swapBlock( (int *)&m_drawSurfaces[0], m_numDrawSurfaces * sizeof( m_drawSurfaces[0] ) );
swapBlock((int *)&m_drawSurfaces[0], m_numDrawSurfaces * sizeof(m_drawSurfaces[0]));
}
bool BspLoader::findVectorByName(float* outvec,const char* name)
bool BspLoader::findVectorByName(float *outvec, const char *name)
{
const char *cl;
BSPVector3 origin;
bool found = false;
parseEntities();
for ( int i = 1; i < m_num_entities; i++ ) {
cl = getValueForKey (&m_entities[i], "classname");
if ( !strcmp( cl, "info_player_start" ) ) {
getVectorForKey( &m_entities[i], "origin", origin );
found = true;
break;
}
if ( !strcmp( cl, "info_player_deathmatch" ) ) {
getVectorForKey( &m_entities[i], "origin", origin );
found = true;
break;
}
for (int i = 1; i < m_num_entities; i++)
{
cl = getValueForKey(&m_entities[i], "classname");
if (!strcmp(cl, "info_player_start"))
{
getVectorForKey(&m_entities[i], "origin", origin);
found = true;
break;
}
if (!strcmp(cl, "info_player_deathmatch"))
{
getVectorForKey(&m_entities[i], "origin", origin);
found = true;
break;
}
}
if (found)
@@ -708,23 +699,21 @@ bool BspLoader::findVectorByName(float* outvec,const char* name)
}
return found;
}
const BSPEntity * BspLoader::getEntityByValue( const char* name, const char* value)
const BSPEntity *BspLoader::getEntityByValue(const char *name, const char *value)
{
const BSPEntity* entity = NULL;
const BSPEntity *entity = NULL;
for ( int i = 1; i < m_num_entities; i++ ) {
for (int i = 1; i < m_num_entities; i++)
{
const BSPEntity &ent = m_entities[i];
const BSPEntity& ent = m_entities[i];
const char* cl = getValueForKey (&m_entities[i], name);
if ( !strcmp( cl, value ) ) {
const char *cl = getValueForKey(&m_entities[i], name);
if (!strcmp(cl, value))
{
entity = &ent;
break;
}
}
return entity;
}

377
examples/Importers/ImportBsp/BspLoader.h
View File

@@ -20,276 +20,269 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
===========================================================================
*/
#ifndef BSP_LOADER_H
#define BSP_LOADER_H
#include "LinearMath/btAlignedObjectArray.h"
#define BSPMAXTOKEN 1024
#define BSPMAX_KEY 32
#define BSPMAX_VALUE 1024
#define BSPCONTENTS_SOLID 1
#define BSPCONTENTS_AREAPORTAL 0x8000
#define BSPLUMP_ENTITIES 0
#define BSPLUMP_SHADERS 1
#define BSPLUMP_PLANES 2
#define BSPLUMP_NODES 3
#define BSPLUMP_LEAFS 4
#define BSPLUMP_LEAFSURFACES 5
#define BSPLUMP_LEAFBRUSHES 6
#define LUMP_MODELS 7
#define LUMP_BRUSHES 8
#define LUMP_BRUSHSIDES 9
#define LUMP_DRAWVERTS 10
#define LUMP_DRAWINDEXES 11
#define LUMP_SURFACES 13
#define LUMP_LIGHTMAPS 14
#define LUMP_LIGHTGRID 15
#define LUMP_VISIBILITY 16
#define HEADER_LUMPS 17
#define MAX_QPATH 64
#define BSPMAXTOKEN 1024
#define BSPMAX_KEY 32
#define BSPMAX_VALUE 1024
#define BSPCONTENTS_SOLID 1
#define BSPCONTENTS_AREAPORTAL 0x8000
#define BSPLUMP_ENTITIES 0
#define BSPLUMP_SHADERS 1
#define BSPLUMP_PLANES 2
#define BSPLUMP_NODES 3
#define BSPLUMP_LEAFS 4
#define BSPLUMP_LEAFSURFACES 5
#define BSPLUMP_LEAFBRUSHES 6
#define LUMP_MODELS 7
#define LUMP_BRUSHES 8
#define LUMP_BRUSHSIDES 9
#define LUMP_DRAWVERTS 10
#define LUMP_DRAWINDEXES 11
#define LUMP_SURFACES 13
#define LUMP_LIGHTMAPS 14
#define LUMP_LIGHTGRID 15
#define LUMP_VISIBILITY 16
#define HEADER_LUMPS 17
#define MAX_QPATH 64
typedef struct {
int fileofs, filelen;
typedef struct
{
int fileofs, filelen;
} BSPLump;
typedef float BSPVector3[3];
typedef struct {
int ident;
int version;
BSPLump lumps[HEADER_LUMPS];
typedef struct
{
int ident;
int version;
BSPLump lumps[HEADER_LUMPS];
} BSPHeader;
typedef struct {
float mins[3], maxs[3];
int firstSurface, numSurfaces;
int firstBrush, numBrushes;
typedef struct
{
float mins[3], maxs[3];
int firstSurface, numSurfaces;
int firstBrush, numBrushes;
} BSPModel;
typedef struct {
char shader[MAX_QPATH];
int surfaceFlags;
int contentFlags;
typedef struct
{
char shader[MAX_QPATH];
int surfaceFlags;
int contentFlags;
} BSPShader;
typedef struct {
float normal[3];
float dist;
typedef struct
{
float normal[3];
float dist;
} BSPPlane;
typedef struct {
int planeNum;
int children[2];
int mins[3];
int maxs[3];
typedef struct
{
int planeNum;
int children[2];
int mins[3];
int maxs[3];
} BSPNode;
typedef struct {
int cluster;
int area;
int mins[3];
int maxs[3];
int firstLeafSurface;
int numLeafSurfaces;
int firstLeafBrush;
int numLeafBrushes;
typedef struct
{
int cluster;
int area;
int mins[3];
int maxs[3];
int firstLeafSurface;
int numLeafSurfaces;
int firstLeafBrush;
int numLeafBrushes;
} BSPLeaf;
typedef struct {
int planeNum;
int shaderNum;
typedef struct
{
int planeNum;
int shaderNum;
} BSPBrushSide;
typedef struct {
int firstSide;
int numSides;
int shaderNum;
typedef struct
{
int firstSide;
int numSides;
int shaderNum;
} BSPBrush;
typedef struct BSPPair {
struct BSPPair *next;
char *key;
char *value;
typedef struct BSPPair
{
struct BSPPair *next;
char *key;
char *value;
} BSPKeyValuePair;
typedef struct {
BSPVector3 origin;
struct bspbrush_s *brushes;
struct parseMesh_s *patches;
int firstDrawSurf;
BSPKeyValuePair *epairs;
typedef struct
{
BSPVector3 origin;
struct bspbrush_s *brushes;
struct parseMesh_s *patches;
int firstDrawSurf;
BSPKeyValuePair *epairs;
} BSPEntity;
typedef enum {
typedef enum
{
MST_BAD,
MST_PLANAR,
MST_PATCH,
MST_TRIANGLE_SOUP,
MST_FLARE
MST_PLANAR,
MST_PATCH,
MST_TRIANGLE_SOUP,
MST_FLARE
} BSPMapSurface;
typedef struct {
int shaderNum;
int fogNum;
int surfaceType;
int firstVert;
int numVerts;
int firstIndex;
int numIndexes;
int lightmapNum;
int lightmapX, lightmapY;
int lightmapWidth, lightmapHeight;
BSPVector3 lightmapOrigin;
BSPVector3 lightmapVecs[3];
int patchWidth;
int patchHeight;
typedef struct
{
int shaderNum;
int fogNum;
int surfaceType;
int firstVert;
int numVerts;
int firstIndex;
int numIndexes;
int lightmapNum;
int lightmapX, lightmapY;
int lightmapWidth, lightmapHeight;
BSPVector3 lightmapOrigin;
BSPVector3 lightmapVecs[3];
int patchWidth;
int patchHeight;
} BSPSurface;
///GPL code from IdSofware to parse a Quake 3 BSP file
///check that your platform define __BIG_ENDIAN__ correctly (in BspLoader.cpp)
class BspLoader
{
int m_Endianness;
public:
public:
BspLoader();
BspLoader();
bool loadBSPFile(void *memoryBuffer);
bool loadBSPFile( void* memoryBuffer);
const char *getValueForKey(const BSPEntity *ent, const char *key) const;
const char* getValueForKey( const BSPEntity *ent, const char *key ) const;
bool getVectorForKey(const BSPEntity *ent, const char *key, BSPVector3 vec);
bool getVectorForKey( const BSPEntity *ent, const char *key, BSPVector3 vec );
float getFloatForKey( const BSPEntity *ent, const char *key );
float getFloatForKey(const BSPEntity *ent, const char *key);
void parseEntities( void );
void parseEntities(void);
bool findVectorByName(float* outvec,const char* name);
bool findVectorByName(float *outvec, const char *name);
const BSPEntity * getEntityByValue( const char* name, const char* value);
const BSPEntity *getEntityByValue(const char *name, const char *value);
protected:
void parseFromMemory(char *buffer, int size);
protected:
bool isEndOfScript(bool crossline);
void parseFromMemory (char *buffer, int size);
bool getToken(bool crossline);
char *copystring(const char *s);
bool isEndOfScript (bool crossline);
void stripTrailing(char *e);
bool getToken (bool crossline);
BSPKeyValuePair *parseEpair(void);
char *copystring(const char *s);
void stripTrailing( char *e );
bool parseEntity(void);
BSPKeyValuePair * parseEpair( void );
short isLittleShort(short l);
int isLittleLong(int l);
float isLittleFloat(float l);
bool parseEntity( void );
int isBigLong(int l);
short isBigShort(short l);
float isBigFloat(float l);
short isLittleShort (short l);
int isLittleLong (int l);
float isLittleFloat (float l);
void swapBlock(int *block, int sizeOfBlock);
int isBigLong (int l);
short isBigShort (short l);
float isBigFloat (float l);
int copyLump(BSPHeader *header, int lump, void *dest, int size);
void swapBlock( int *block, int sizeOfBlock );
void swapBSPFile(void);
int copyLump( BSPHeader *header, int lump, void *dest, int size );
public: //easier for conversion
int m_num_entities;
btAlignedObjectArray<BSPEntity> m_entities;
void swapBSPFile( void );
int m_nummodels;
btAlignedObjectArray<BSPModel> m_dmodels;
public: //easier for conversion
int m_num_entities;
btAlignedObjectArray<BSPEntity> m_entities;
int m_nummodels;
btAlignedObjectArray<BSPModel> m_dmodels;
int m_numShaders;
btAlignedObjectArray<BSPShader> m_dshaders;
int m_numShaders;
btAlignedObjectArray<BSPShader> m_dshaders;
int m_entdatasize;
btAlignedObjectArray<char> m_dentdata;
int m_entdatasize;
btAlignedObjectArray<char> m_dentdata;
int m_numleafs;
btAlignedObjectArray<BSPLeaf> m_dleafs;
int m_numleafs;
btAlignedObjectArray<BSPLeaf> m_dleafs;
int m_numplanes;
btAlignedObjectArray<BSPPlane> m_dplanes;
int m_numplanes;
btAlignedObjectArray<BSPPlane> m_dplanes;
int m_numnodes;
btAlignedObjectArray<BSPNode> m_dnodes;
int m_numnodes;
btAlignedObjectArray<BSPNode> m_dnodes;
int m_numleafsurfaces;
btAlignedObjectArray<int> m_dleafsurfaces;
int m_numleafsurfaces;
btAlignedObjectArray<int> m_dleafsurfaces;
int m_numleafbrushes;
btAlignedObjectArray<int> m_dleafbrushes;
int m_numleafbrushes;
btAlignedObjectArray<int> m_dleafbrushes;
int m_numbrushes;
btAlignedObjectArray<BSPBrush> m_dbrushes;
int m_numbrushes;
btAlignedObjectArray<BSPBrush> m_dbrushes;
int m_numbrushsides;
btAlignedObjectArray<BSPBrushSide> m_dbrushsides;
int m_numbrushsides;
btAlignedObjectArray<BSPBrushSide> m_dbrushsides;
int m_numLightBytes;
btAlignedObjectArray<unsigned char> m_lightBytes;
int m_numLightBytes;
btAlignedObjectArray<unsigned char> m_lightBytes;
int m_numGridPoints;
btAlignedObjectArray<unsigned char> m_gridData;
int m_numGridPoints;
btAlignedObjectArray<unsigned char> m_gridData;
int m_numVisBytes;
btAlignedObjectArray<unsigned char> m_visBytes;
int m_numVisBytes;
btAlignedObjectArray<unsigned char> m_visBytes;
int m_numDrawIndexes;
btAlignedObjectArray<int> m_drawIndexes;
int m_numDrawIndexes;
btAlignedObjectArray<int> m_drawIndexes;
int m_numDrawSurfaces;
btAlignedObjectArray<BSPSurface> m_drawSurfaces;
int m_numDrawSurfaces;
btAlignedObjectArray<BSPSurface> m_drawSurfaces;
enum
{
BSP_LITTLE_ENDIAN = 0,
BSP_BIG_ENDIAN = 1
};
enum
{
BSP_LITTLE_ENDIAN = 0,
BSP_BIG_ENDIAN = 1
};
//returns machines big endian / little endian
//
int getMachineEndianness();
inline int machineEndianness()
{
return m_Endianness;
}
//returns machines big endian / little endian
//
int getMachineEndianness();
inline int machineEndianness()
{
return m_Endianness;
}
};
#endif //BSP_LOADER_H
#endif //BSP_LOADER_H

189
examples/Importers/ImportBsp/ImportBspExample.cpp
View File

@@ -19,198 +19,163 @@ subject to the following restrictions:
#include "LinearMath/btQuickprof.h"
#define QUAKE_BSP_IMPORTING 1
#ifdef QUAKE_BSP_IMPORTING
#include "BspLoader.h"
#include "BspConverter.h"
#endif //QUAKE_BSP_IMPORTING
#include <stdio.h> //printf debugging
#endif //QUAKE_BSP_IMPORTING
#include <stdio.h> //printf debugging
#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
///BspDemo shows the convex collision detection, by converting a Quake BSP file into convex objects and allowing interaction with boxes.
class BspDemo : public CommonRigidBodyBase
{
public:
public:
//keep the collision shapes, for deletion/cleanup
BspDemo(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
}
virtual ~BspDemo();
virtual void initPhysics();
virtual void initPhysics();
void initPhysics(const char* bspfilename);
void initPhysics(const char* bspfilename);
virtual void resetCamera()
{
float dist = 43;
float pitch = -12;
float yaw = -175;
float targetPos[3]={4,-25,-6};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {4, -25, -6};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
#define CUBE_HALF_EXTENTS 1
#define EXTRA_HEIGHT -20.f
///BspToBulletConverter extends the BspConverter to convert to Bullet datastructures
class BspToBulletConverter : public BspConverter
{
BspDemo* m_demoApp;
public:
BspToBulletConverter(BspDemo* demoApp)
:m_demoApp(demoApp)
BspToBulletConverter(BspDemo* demoApp)
: m_demoApp(demoApp)
{
}
virtual void addConvexVerticesCollider(btAlignedObjectArray<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation)
virtual void addConvexVerticesCollider(btAlignedObjectArray<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation)
{
///perhaps we can do something special with entities (isEntity)
///like adding a collision Triggering (as example)
if (vertices.size() > 0)
{
///perhaps we can do something special with entities (isEntity)
///like adding a collision Triggering (as example)
float mass = 0.f;
btTransform startTransform;
//can use a shift
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0, 0, -10.f));
//this create an internal copy of the vertices
if (vertices.size() > 0)
{
float mass = 0.f;
btTransform startTransform;
//can use a shift
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,0,-10.f));
//this create an internal copy of the vertices
btCollisionShape* shape = new btConvexHullShape(&(vertices[0].getX()), vertices.size());
m_demoApp->m_collisionShapes.push_back(shape);
btCollisionShape* shape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
m_demoApp->m_collisionShapes.push_back(shape);
//btRigidBody* body = m_demoApp->localCreateRigidBody(mass, startTransform,shape);
m_demoApp->createRigidBody(mass, startTransform,shape);
}
//btRigidBody* body = m_demoApp->localCreateRigidBody(mass, startTransform,shape);
m_demoApp->createRigidBody(mass, startTransform, shape);
}
}
};
////////////////////////////////////
BspDemo::~BspDemo()
{
exitPhysics(); //will delete all default data
exitPhysics(); //will delete all default data
}
void BspDemo::initPhysics()
void BspDemo::initPhysics()
{
const char* bspfilename = "BspDemo.bsp";
initPhysics(bspfilename);
}
void BspDemo::initPhysics(const char* bspfilename)
void BspDemo::initPhysics(const char* bspfilename)
{
int cameraUpAxis =2;
int cameraUpAxis = 2;
m_guiHelper->setUpAxis(cameraUpAxis);
btVector3 grav(0,0,0);
btVector3 grav(0, 0, 0);
grav[cameraUpAxis] = -10;
m_guiHelper->setUpAxis(cameraUpAxis);
//_cameraUp = btVector3(0,0,1);
//_forwardAxis = 1;
//etCameraDistance(22.f);
//_cameraUp = btVector3(0,0,1);
//_forwardAxis = 1;
//etCameraDistance(22.f);
///Setup a Physics Simulation Environment
m_collisionConfiguration = new btDefaultCollisionConfiguration();
// btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
// btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btVector3 worldMin(-1000, -1000, -1000);
btVector3 worldMax(1000, 1000, 1000);
m_broadphase = new btDbvtBroadphase();
//m_broadphase = new btAxisSweep3(worldMin,worldMax);
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
m_solver = new btSequentialImpulseConstraintSolver();
//ConstraintSolver* solver = new OdeConstraintSolver;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(grav);
#ifdef QUAKE_BSP_IMPORTING
void* memoryBuffer = 0;
const char* filename = "BspDemo.bsp";
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
FILE* file=0;
for (int i=0;i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],filename);
file = fopen(relativeFileName,"r");
if (file)
break;
}
const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
char relativeFileName[1024];
FILE* file = 0;
for (int i = 0; i < numPrefixes; i++)
{
sprintf(relativeFileName, "%s%s", prefix[i], filename);
file = fopen(relativeFileName, "r");
if (file)
break;
}
if (file)
{
BspLoader bspLoader;
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) { /* File operations denied? ok, just close and return failure */
int size = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{ /* File operations denied? ok, just close and return failure */
printf("Error: cannot get filesize from %s\n", bspfilename);
} else
}
else
{
//how to detect file size?
memoryBuffer = malloc(size+1);
fread(memoryBuffer,1,size,file);
bspLoader.loadBSPFile( memoryBuffer);
memoryBuffer = malloc(size + 1);
fread(memoryBuffer, 1, size, file);
bspLoader.loadBSPFile(memoryBuffer);
BspToBulletConverter bsp2bullet(this);
float bspScaling = 0.1f;
bsp2bullet.convertBsp(bspLoader,bspScaling);
bsp2bullet.convertBsp(bspLoader, bspScaling);
}
fclose(file);
}
@@ -218,18 +183,8 @@ void BspDemo::initPhysics(const char* bspfilename)
#endif
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
//some code that de-mangles the windows filename passed in as argument
char cleaned_filename[512];
char* getLastFileName()
@@ -238,33 +193,31 @@ char* getLastFileName()
}
char* makeExeToBspFilename(const char* lpCmdLine)
{
// We might get a windows-style path on the command line, this can mess up the DOM which expects
// all paths to be URI's. This block of code does some conversion to try and make the input
// compliant without breaking the ability to accept a properly formatted URI. Right now this only
// displays the first filename
const char *in = lpCmdLine;
const char* in = lpCmdLine;
char* out = cleaned_filename;
*out = '\0';
// If the first character is a ", skip it (filenames with spaces in them are quoted)
if(*in == '\"')
if (*in == '\"')
{
in++;
}
int i;
for(i =0; i<512; i++)
for (i = 0; i < 512; i++)
{
//if we get '.' we stop as well, unless it's the first character. Then we add .bsp as extension
// If we hit a null or a quote, stop copying. This will get just the first filename.
if(i && (in[0] == '.') && (in[1] == 'e') && (in[2] == 'x') && (in[3] == 'e'))
if (i && (in[0] == '.') && (in[1] == 'e') && (in[2] == 'x') && (in[3] == 'e'))
break;
// If we hit a null or a quote, stop copying. This will get just the first filename.
if(*in == '\0' || *in == '\"')
if (*in == '\0' || *in == '\"')
break;
// Copy while swapping backslashes for forward ones
if(*in == '\\')
if (*in == '\\')
{
*out = '/';
}
@@ -284,14 +237,12 @@ char* makeExeToBspFilename(const char* lpCmdLine)
return cleaned_filename;
}
CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options)
CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options)
{
BspDemo* demo = new BspDemo(options.m_guiHelper);
demo->initPhysics("BspDemo.bsp");
return demo;
demo->initPhysics("BspDemo.bsp");
return demo;
}
/*
static DemoApplication* Create()

7
examples/Importers/ImportBsp/ImportBspExample.h
View File

@@ -15,9 +15,6 @@ subject to the following restrictions:
#ifndef BSP_DEMO_H
#define BSP_DEMO_H
class CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options);
#endif //BSP_DEMO_H
class CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options);
#endif //BSP_DEMO_H

82
examples/Importers/ImportBullet/SerializeSetup.cpp
View File

@@ -1,114 +1,104 @@
#include "SerializeSetup.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
class SerializeSetup : public CommonRigidBodyBase
{
char m_fileName[1024];
public:
SerializeSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~SerializeSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void setFileName(const char* fileName)
{
memcpy(m_fileName,fileName,strlen(fileName)+1);
memcpy(m_fileName, fileName, strlen(fileName) + 1);
}
virtual void resetCamera()
{
float dist = 9.5;
float pitch = -20;
float yaw = -2.8;
float targetPos[3]={-0.2,-1.4,3.5};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {-0.2, -1.4, 3.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
SerializeSetup::SerializeSetup(struct GUIHelperInterface* helper, const char* fileName)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
if (fileName)
{
setFileName(fileName);
} else
}
else
{
setFileName("spider.bullet");
}
}
SerializeSetup::~SerializeSetup()
{
}
void SerializeSetup::initPhysics()
{
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld);
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld);
const char* prefix[]={"","./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
FILE* f=0;
const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
char relativeFileName[1024];
FILE* f = 0;
for (int i=0;!f && i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],m_fileName);
f = fopen(relativeFileName,"rb");
if (f)
{
break;
}
}
if (f)
{
fclose(f);
}
for (int i = 0; !f && i < numPrefixes; i++)
{
sprintf(relativeFileName, "%s%s", prefix[i], m_fileName);
f = fopen(relativeFileName, "rb");
if (f)
{
break;
}
}
if (f)
{
fclose(f);
}
importer->loadFile(relativeFileName);
importer->loadFile(relativeFileName);
//for now, guess the up axis from gravity
if (m_dynamicsWorld->getGravity()[1] == 0.f)
{
m_guiHelper->setUpAxis(2);
} else
}
else
{
m_guiHelper->setUpAxis(1);
}
//example code to export the dynamics world to a .bullet file
btDefaultSerializer* serializer = new btDefaultSerializer();
btDefaultSerializer* serializer = new btDefaultSerializer();
m_dynamicsWorld->serialize(serializer);
FILE* file = fopen("SerializeSetupTestFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file);
FILE* file = fopen("SerializeSetupTestFile.bullet", "wb");
fwrite(serializer->getBufferPointer(), serializer->getCurrentBufferSize(), 1, file);
fclose(file);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void SerializeSetup::stepSimulation(float deltaTime)
{
CommonRigidBodyBase::stepSimulation(deltaTime);
CommonRigidBodyBase::stepSimulation(deltaTime);
}
class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options)
{
return new SerializeSetup(options.m_guiHelper, options.m_fileName);
}

5
examples/Importers/ImportBullet/SerializeSetup.h
View File

@@ -1,7 +1,6 @@
#ifndef SERIALIZE_SETUP_H
#define SERIALIZE_SETUP_H
class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options);
#endif //SERIALIZE_SETUP_H
#endif //SERIALIZE_SETUP_H

8
examples/Importers/ImportColladaDemo/ColladaGraphicsInstance.h
View File

@@ -23,13 +23,13 @@ subject to the following restrictions:
struct ColladaGraphicsInstance
{
ColladaGraphicsInstance()
:m_shapeIndex(-1)
: m_shapeIndex(-1)
{
m_worldTransform.setIdentity();
}
btMatrix4x4 m_worldTransform;
int m_shapeIndex;//could be index into array of GLInstanceGraphicsShape
btMatrix4x4 m_worldTransform;
int m_shapeIndex; //could be index into array of GLInstanceGraphicsShape
float m_color[4];
};
#endif //COLLADA_GRAPHICS_INSTANCE_H
#endif //COLLADA_GRAPHICS_INSTANCE_H

120
examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp
View File

@@ -15,7 +15,6 @@ subject to the following restrictions:
//original author: Erwin Coumans
*/
#include "ImportColladaSetup.h"
#include <vector>
#include "../OpenGLWindow/GLInstancingRenderer.h"
@@ -28,49 +27,42 @@ subject to the following restrictions:
#include "../CommonInterfaces/CommonRigidBodyBase.h"
class ImportColladaSetup : public CommonRigidBodyBase
{
public:
ImportColladaSetup(struct GUIHelperInterface* helper);
virtual ~ImportColladaSetup();
ImportColladaSetup(struct GUIHelperInterface* helper);
virtual ~ImportColladaSetup();
virtual void initPhysics();
virtual void resetCamera()
{
float dist = 16;
float pitch = -28;
float yaw = -140;
float targetPos[3]={-4,-3,-3};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {-4, -3, -3};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
ImportColladaSetup::ImportColladaSetup(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
}
ImportColladaSetup::~ImportColladaSetup()
{
}
static int ColladaGraphicsInstanceSortfnc(const ColladaGraphicsInstance& a,const ColladaGraphicsInstance& b)
static int ColladaGraphicsInstanceSortfnc(const ColladaGraphicsInstance& a, const ColladaGraphicsInstance& b)
{
if (a.m_shapeIndex<b.m_shapeIndex) return +1;
if (a.m_shapeIndex>b.m_shapeIndex) return -1;
if (a.m_shapeIndex < b.m_shapeIndex) return +1;
if (a.m_shapeIndex > b.m_shapeIndex) return -1;
return 0;
}
void ImportColladaSetup::initPhysics()
{
int upAxis=1;
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -81,28 +73,20 @@ void ImportColladaSetup::initPhysics()
const char* fileNames[] = {
"duck.dae",
"seymourplane_triangulate.dae",
};
};
const char* fileName = fileNames[fileIndex];
int numFiles = sizeof(fileNames)/sizeof(const char*);
char relativeFileName[1024];
int numFiles = sizeof(fileNames) / sizeof(const char*);
if (!b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))
return;
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
return;
btVector3 shift(0,0,0);
btVector3 scaling(1,1,1);
// int index=10;
btVector3 shift(0, 0, 0);
btVector3 scaling(1, 1, 1);
// int index=10;
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -110,69 +94,64 @@ void ImportColladaSetup::initPhysics()
btTransform upAxisTrans;
upAxisTrans.setIdentity();
btVector3 color(0,0,1);
btVector3 color(0, 0, 1);
#ifdef COMPARE_WITH_ASSIMP
static int useAssimp = 0;
if (useAssimp)
{
LoadMeshFromColladaAssimp(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling);
LoadMeshFromColladaAssimp(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling);
fileIndex++;
if (fileIndex>=numFiles)
if (fileIndex >= numFiles)
{
fileIndex = 0;
}
color.setValue(1,0,0);
color.setValue(1, 0, 0);
}
else
{
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling);
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling);
}
useAssimp=1-useAssimp;
useAssimp = 1 - useAssimp;
#else
fileIndex++;
if (fileIndex>=numFiles)
if (fileIndex >= numFiles)
{
fileIndex = 0;
}
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling, upAxis);
#endif// COMPARE_WITH_ASSIMP
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis);
#endif // COMPARE_WITH_ASSIMP
//at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape
//so perform a sort, just to be sure
visualShapeInstances.quickSort(ColladaGraphicsInstanceSortfnc);
for (int i=0;i<visualShapeInstances.size();i++)
for (int i = 0; i < visualShapeInstances.size(); i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
btVector3 position(0,0,0);// = scaling*btVector3(instance->m_pos[0],instance->m_pos[1],instance->m_pos[2]);
btQuaternion orn(0,0,0,1);//instance->m_orn[0],instance->m_orn[1],instance->m_orn[2],instance->m_orn[3]);
btVector3 position(0, 0, 0); // = scaling*btVector3(instance->m_pos[0],instance->m_pos[1],instance->m_pos[2]);
btQuaternion orn(0, 0, 0, 1); //instance->m_orn[0],instance->m_orn[1],instance->m_orn[2],instance->m_orn[3]);
//sort the visualShapeInstances, then iterate etc
//void LoadMeshFromCollada(const char* relativeFileName,
//btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
//btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
//btAlignedObjectArray<GLInstanceGraphicsInstance> visualShapeInstances);
if (gfxShape)
{
//btTransform trans;
//trans.setIdentity();
//trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
for (int i=0;i<gfxShape->m_vertices->size();i++)
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].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];
@@ -180,34 +159,33 @@ void ImportColladaSetup::initPhysics()
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
}
//compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat;
upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat * upAxisMat * instance->m_worldTransform;
//btMatrix4x4 worldMat = instance->m_worldTransform;
for(int v=0;v<verts.size();v++)
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;
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]);
}
int shapeId = m_guiHelper->getRenderInterface()->registerShape(&verts[0].xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
//btVector3 instanceScaling(instance->m_scaling[0],instance->m_scaling[1],instance->m_scaling[2]);
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, position, orn, color, scaling);
}
}
}
}
class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options)
{
return new ImportColladaSetup(options.m_guiHelper);
}

6
examples/Importers/ImportColladaDemo/ImportColladaSetup.h
View File

@@ -15,11 +15,9 @@ subject to the following restrictions:
//original author: Erwin Coumans
*/
#ifndef IMPORT_COLLADA_SETUP_H
#define IMPORT_COLLADA_SETUP_H
class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_COLLADA_SETUP_H
#endif //IMPORT_COLLADA_SETUP_H

588
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -15,9 +15,8 @@ subject to the following restrictions:
//original author: Erwin Coumans
*/
#include "LoadMeshFromCollada.h"
#include <stdio.h> //fopen
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
@@ -28,10 +27,8 @@ using namespace tinyxml2;
#include <assert.h>
#include "btMatrix4x4.h"
#define MAX_VISUAL_SHAPES 512
struct VertexSource
{
std::string m_positionArrayId;
@@ -42,7 +39,8 @@ struct TokenFloatArray
{
btAlignedObjectArray<float>& m_values;
TokenFloatArray(btAlignedObjectArray<float>& floatArray)
:m_values(floatArray) {
: m_values(floatArray)
{
}
inline void add(const char* token)
{
@@ -54,7 +52,8 @@ struct TokenIntArray
{
btAlignedObjectArray<int>& m_values;
TokenIntArray(btAlignedObjectArray<int>& intArray)
:m_values(intArray) {
: m_values(intArray)
{
}
inline void add(const char* token)
{
@@ -66,127 +65,125 @@ struct TokenIntArray
template <typename AddToken>
void tokenize(const std::string& str, AddToken& tokenAdder, const std::string& delimiters = " ")
{
std::string::size_type pos, lastPos = 0;
while(true)
{
pos = str.find_first_of(delimiters, lastPos);
if(pos == std::string::npos)
{
pos = str.length();
if(pos != lastPos)
{
tokenAdder.add(str.data()+lastPos);
}
break;
}
else
{
if(pos != lastPos)
{
tokenAdder.add(str.data()+lastPos);
}
}
lastPos = pos + 1;
}
std::string::size_type pos, lastPos = 0;
while (true)
{
pos = str.find_first_of(delimiters, lastPos);
if (pos == std::string::npos)
{
pos = str.length();
if (pos != lastPos)
{
tokenAdder.add(str.data() + lastPos);
}
break;
}
else
{
if (pos != lastPos)
{
tokenAdder.add(str.data() + lastPos);
}
}
lastPos = pos + 1;
}
}
void readFloatArray(XMLElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride)
void readFloatArray(XMLElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride)
{
int numVals, stride;
XMLElement* array = source->FirstChildElement("float_array");
if(array)
if (array)
{
componentStride = 1;
if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride)!= XML_NO_ATTRIBUTE)
if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride) != XML_NO_ATTRIBUTE)
{
componentStride = stride;
}
array->QueryIntAttribute("count", &numVals);
TokenFloatArray adder(floatArray);
floatArray.reserve(numVals);
tokenize(array->GetText(),adder);
tokenize(array->GetText(), adder);
assert(floatArray.size() == numVals);
}
}
btVector3 getVector3FromXmlText(const char* text)
{
btVector3 vec(0,0,0);
btVector3 vec(0, 0, 0);
btAlignedObjectArray<float> floatArray;
TokenFloatArray adder(floatArray);
floatArray.reserve(3);
tokenize(text,adder);
tokenize(text, adder);
assert(floatArray.size() == 3);
if (floatArray.size()==3)
if (floatArray.size() == 3)
{
vec.setValue(floatArray[0],floatArray[1],floatArray[2]);
vec.setValue(floatArray[0], floatArray[1], floatArray[2]);
}
return vec;
}
btVector4 getVector4FromXmlText(const char* text)
{
btVector4 vec(0,0,0,0);
btVector4 vec(0, 0, 0, 0);
btAlignedObjectArray<float> floatArray;
TokenFloatArray adder(floatArray);
floatArray.reserve(4);
tokenize(text,adder);
tokenize(text, adder);
assert(floatArray.size() == 4);
if (floatArray.size()==4)
if (floatArray.size() == 4)
{
vec.setValue(floatArray[0],floatArray[1],floatArray[2],floatArray[3]);
vec.setValue(floatArray[0], floatArray[1], floatArray[2], floatArray[3]);
}
return vec;
}
void readLibraryGeometries(XMLDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling)
void readLibraryGeometries(XMLDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString, int>& name2Shape, float extraScaling)
{
btHashMap<btHashString,XMLElement* > allSources;
btHashMap<btHashString,VertexSource> vertexSources;
for(XMLElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry");
geometry != NULL; geometry = geometry->NextSiblingElement("geometry"))
btHashMap<btHashString, XMLElement*> allSources;
btHashMap<btHashString, VertexSource> vertexSources;
for (XMLElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry");
geometry != NULL; geometry = geometry->NextSiblingElement("geometry"))
{
btAlignedObjectArray<btVector3> vertexPositions;
btAlignedObjectArray<btVector3> vertexNormals;
btAlignedObjectArray<int> indices;
const char* geometryName = geometry->Attribute("id");
for (XMLElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
for (XMLElement* mesh = geometry->FirstChildElement("mesh"); (mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
{
XMLElement* vertices2 = mesh->FirstChildElement("vertices");
for (XMLElement* source = mesh->FirstChildElement("source");source != NULL;source = source->NextSiblingElement("source"))
for (XMLElement* source = mesh->FirstChildElement("source"); source != NULL; source = source->NextSiblingElement("source"))
{
const char* srcId= source->Attribute("id");
// printf("source id=%s\n",srcId);
allSources.insert(srcId,source);
const char* srcId = source->Attribute("id");
// printf("source id=%s\n",srcId);
allSources.insert(srcId, source);
}
const char* vertexId = vertices2->Attribute("id");
//printf("vertices id=%s\n",vertexId);
VertexSource vs;
for(XMLElement* input = vertices2->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
for (XMLElement* input = vertices2->FirstChildElement("input"); input != NULL; input = input->NextSiblingElement("input"))
{
const char* sem = input->Attribute("semantic");
std::string semName(sem);
// printf("sem=%s\n",sem);
// const char* src = input->Attribute("source");
// printf("src=%s\n",src);
// printf("sem=%s\n",sem);
// const char* src = input->Attribute("source");
// printf("src=%s\n",src);
const char* srcIdRef = input->Attribute("source");
std::string source_name;
source_name = std::string(srcIdRef);
source_name = source_name.erase(0, 1);
if (semName=="POSITION")
if (semName == "POSITION")
{
vs.m_positionArrayId = source_name;
}
if (semName=="NORMAL")
if (semName == "NORMAL")
{
vs.m_normalArrayId = source_name;
}
}
vertexSources.insert(vertexId,vs);
vertexSources.insert(vertexId, vs);
btAlignedObjectArray<XMLElement*> trianglesAndPolylists;
@@ -199,36 +196,35 @@ void readLibraryGeometries(XMLDocument& doc, btAlignedObjectArray<GLInstanceGrap
trianglesAndPolylists.push_back(primitive);
}
for (int i=0;i<trianglesAndPolylists.size();i++)
for (int i = 0; i < trianglesAndPolylists.size(); i++)
{
XMLElement* primitive = trianglesAndPolylists[i];
std::string positionSourceName;
std::string normalSourceName;
int primitiveCount;
primitive->QueryIntAttribute("count", &primitiveCount);
int indexStride=1;
int indexStride = 1;
int posOffset = 0;
int normalOffset = 0;
int numIndices = 0;
{
for (XMLElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
for (XMLElement* input = primitive->FirstChildElement("input"); input != NULL; input = input->NextSiblingElement("input"))
{
const char* sem = input->Attribute("semantic");
std::string semName(sem);
int offset = atoi(input->Attribute("offset"));
if ((offset+1)>indexStride)
indexStride=offset+1;
if ((offset + 1) > indexStride)
indexStride = offset + 1;
//printf("sem=%s\n",sem);
// const char* src = input->Attribute("source");
// const char* src = input->Attribute("source");
//printf("src=%s\n",src);
const char* srcIdRef = input->Attribute("source");
std::string source_name;
source_name = std::string(srcIdRef);
source_name = source_name.erase(0, 1);
if (semName=="VERTEX")
if (semName == "VERTEX")
{
//now we have POSITION and possibly NORMAL too, using same index array (<p>)
VertexSource* vs = vertexSources[source_name.c_str()];
@@ -240,138 +236,138 @@ void readLibraryGeometries(XMLDocument& doc, btAlignedObjectArray<GLInstanceGrap
if (vs->m_normalArrayId.length())
{
normalSourceName = vs->m_normalArrayId;
normalOffset = offset;
normalOffset = offset;
}
}
if (semName=="NORMAL")
if (semName == "NORMAL")
{
btAssert(normalSourceName.length()==0);
btAssert(normalSourceName.length() == 0);
normalSourceName = source_name;
normalOffset = offset;
normalOffset = offset;
}
}
numIndices = primitiveCount * 3;
numIndices = primitiveCount * 3;
}
btAlignedObjectArray<float> positionFloatArray;
int posStride=1;
int posStride = 1;
XMLElement** sourcePtr = allSources[positionSourceName.c_str()];
if (sourcePtr)
{
readFloatArray(*sourcePtr,positionFloatArray, posStride);
readFloatArray(*sourcePtr, positionFloatArray, posStride);
}
btAlignedObjectArray<float> normalFloatArray;
int normalStride=1;
int normalStride = 1;
sourcePtr = allSources[normalSourceName.c_str()];
if (sourcePtr)
{
readFloatArray(*sourcePtr,normalFloatArray,normalStride);
readFloatArray(*sourcePtr, normalFloatArray, normalStride);
}
btAlignedObjectArray<int> curIndices;
curIndices.reserve(numIndices*indexStride);
curIndices.reserve(numIndices * indexStride);
TokenIntArray adder(curIndices);
tokenize(primitive->FirstChildElement("p")->GetText(),adder);
assert(curIndices.size() == numIndices*indexStride);
tokenize(primitive->FirstChildElement("p")->GetText(), adder);
assert(curIndices.size() == numIndices * indexStride);
int indexOffset = vertexPositions.size();
for(int index=0; index<numIndices; index++)
for (int index = 0; index < numIndices; index++)
{
int posIndex = curIndices[index*indexStride+posOffset];
int normalIndex = curIndices[index*indexStride+normalOffset];
vertexPositions.push_back(btVector3(extraScaling*positionFloatArray[posIndex*3+0],
extraScaling*positionFloatArray[posIndex*3+1],
extraScaling*positionFloatArray[posIndex*3+2]));
if (normalFloatArray.size() && (normalFloatArray.size()>normalIndex))
int posIndex = curIndices[index * indexStride + posOffset];
int normalIndex = curIndices[index * indexStride + normalOffset];
vertexPositions.push_back(btVector3(extraScaling * positionFloatArray[posIndex * 3 + 0],
extraScaling * positionFloatArray[posIndex * 3 + 1],
extraScaling * positionFloatArray[posIndex * 3 + 2]));
if (normalFloatArray.size() && (normalFloatArray.size() > normalIndex))
{
vertexNormals.push_back(btVector3(normalFloatArray[normalIndex*3+0],
normalFloatArray[normalIndex*3+1],
normalFloatArray[normalIndex*3+2]));
} else
vertexNormals.push_back(btVector3(normalFloatArray[normalIndex * 3 + 0],
normalFloatArray[normalIndex * 3 + 1],
normalFloatArray[normalIndex * 3 + 2]));
}
else
{
//add a dummy normal of length zero, so it is easy to detect that it is an invalid normal
vertexNormals.push_back(btVector3(0,0,0));
vertexNormals.push_back(btVector3(0, 0, 0));
}
}
int curNumIndices = indices.size();
indices.resize(curNumIndices+numIndices);
for(int index=0; index<numIndices; index++)
indices.resize(curNumIndices + numIndices);
for (int index = 0; index < numIndices; index++)
{
indices[curNumIndices+index] = index+indexOffset;
indices[curNumIndices + index] = index + indexOffset;
}
}//if(primitive != NULL)
}//for each mesh
} //if(primitive != NULL)
} //for each mesh
int shapeIndex = visualShapes.size();
if (shapeIndex<MAX_VISUAL_SHAPES)
{
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
if (shapeIndex < MAX_VISUAL_SHAPES)
{
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
btAssert(vertexNormals.size()==vertexPositions.size());
for (int v=0;v<vertexPositions.size();v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
btAssert(vertexNormals.size() == vertexPositions.size());
for (int v = 0; v < vertexPositions.size(); v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
for (int index=0;index<indices.size();index++)
{
visualShape.m_indices->push_back(indices[index]+indexBase);
}
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase=visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName,shapeIndex);
} else
{
b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES);
}
for (int index = 0; index < indices.size(); index++)
{
visualShape.m_indices->push_back(indices[index] + indexBase);
}
}//for each geometry
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase = visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName, shapeIndex);
}
else
{
b3Warning("DAE exceeds number of visual shapes (%d/%d)", shapeIndex, MAX_VISUAL_SHAPES);
}
} //for each geometry
}
void readNodeHierarchy(XMLElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
void readNodeHierarchy(XMLElement* node, btHashMap<btHashString, int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{
btMatrix4x4 nodeTrans;
nodeTrans.setIdentity();
///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
{
for (XMLElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
for (XMLElement* transElem = node->FirstChildElement("matrix"); transElem; transElem = node->NextSiblingElement("matrix"))
{
if (transElem->GetText())
{
btAlignedObjectArray<float> floatArray;
TokenFloatArray adder(floatArray);
tokenize(transElem->GetText(),adder);
if (floatArray.size()==16)
tokenize(transElem->GetText(), adder);
if (floatArray.size() == 16)
{
btMatrix4x4 t(floatArray[0],floatArray[1],floatArray[2],floatArray[3],
floatArray[4],floatArray[5],floatArray[6],floatArray[7],
floatArray[8],floatArray[9],floatArray[10],floatArray[11],
floatArray[12],floatArray[13],floatArray[14],floatArray[15]);
btMatrix4x4 t(floatArray[0], floatArray[1], floatArray[2], floatArray[3],
floatArray[4], floatArray[5], floatArray[6], floatArray[7],
floatArray[8], floatArray[9], floatArray[10], floatArray[11],
floatArray[12], floatArray[13], floatArray[14], floatArray[15]);
nodeTrans = nodeTrans*t;
} else
nodeTrans = nodeTrans * t;
}
else
{
b3Warning("Error: expected 16 elements in a <matrix> element, skipping\n");
}
@@ -380,7 +376,7 @@ void readNodeHierarchy(XMLElement* node,btHashMap<btHashString,int>& name2Shape,
}
{
for (XMLElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
for (XMLElement* transElem = node->FirstChildElement("translate"); transElem; transElem = node->NextSiblingElement("translate"))
{
if (transElem->GetText())
{
@@ -388,45 +384,44 @@ void readNodeHierarchy(XMLElement* node,btHashMap<btHashString,int>& name2Shape,
//nodePos+= unitScaling*parentScaling*pos;
btMatrix4x4 t;
t.setPureTranslation(pos);
nodeTrans = nodeTrans*t;
nodeTrans = nodeTrans * t;
}
}
}
{
for(XMLElement* scaleElem = node->FirstChildElement("scale");
scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale"))
for (XMLElement* scaleElem = node->FirstChildElement("scale");
scaleElem != NULL; scaleElem = node->NextSiblingElement("scale"))
{
if (scaleElem->GetText())
{
btVector3 scaling = getVector3FromXmlText(scaleElem->GetText());
btMatrix4x4 t;
t.setPureScaling(scaling);
nodeTrans = nodeTrans*t;
nodeTrans = nodeTrans * t;
}
}
}
{
for(XMLElement* rotateElem = node->FirstChildElement("rotate");
rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate"))
for (XMLElement* rotateElem = node->FirstChildElement("rotate");
rotateElem != NULL; rotateElem = node->NextSiblingElement("rotate"))
{
if (rotateElem->GetText())
{
//accumulate orientation
btVector4 rotate = getVector4FromXmlText(rotateElem->GetText());
btQuaternion orn(btVector3(rotate),btRadians(rotate[3]));//COLLADA DAE rotate is in degrees, convert to radians
btQuaternion orn(btVector3(rotate), btRadians(rotate[3])); //COLLADA DAE rotate is in degrees, convert to radians
btMatrix4x4 t;
t.setPureRotation(orn);
nodeTrans = nodeTrans*t;
nodeTrans = nodeTrans * t;
}
}
}
nodeTrans = parentTransMat*nodeTrans;
nodeTrans = parentTransMat * nodeTrans;
for (XMLElement* instanceGeom = node->FirstChildElement("instance_geometry");
instanceGeom!=0;
instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
instanceGeom != 0;
instanceGeom = instanceGeom->NextSiblingElement("instance_geometry"))
{
const char* geomUrl = instanceGeom->Attribute("url");
//printf("node referring to geom %s\n", geomUrl);
@@ -434,37 +429,38 @@ void readNodeHierarchy(XMLElement* node,btHashMap<btHashString,int>& name2Shape,
int* shapeIndexPtr = name2Shape[geomUrl];
if (shapeIndexPtr)
{
// int index = *shapeIndexPtr;
// int index = *shapeIndexPtr;
//printf("found geom with index %d\n", *shapeIndexPtr);
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = *shapeIndexPtr;
instance.m_worldTransform = nodeTrans;
} else
}
else
{
b3Warning("geom not found\n");
}
}
for(XMLElement* childNode = node->FirstChildElement("node");
childNode!= NULL; childNode = childNode->NextSiblingElement("node"))
for (XMLElement* childNode = node->FirstChildElement("node");
childNode != NULL; childNode = childNode->NextSiblingElement("node"))
{
readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans);
readNodeHierarchy(childNode, name2Shape, visualShapeInstances, nodeTrans);
}
}
void readVisualSceneInstanceGeometries(XMLDocument& doc, btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
void readVisualSceneInstanceGeometries(XMLDocument& doc, btHashMap<btHashString, int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
{
btHashMap<btHashString,XMLElement* > allVisualScenes;
btHashMap<btHashString, XMLElement*> allVisualScenes;
XMLElement* libVisualScenes = doc.RootElement()->FirstChildElement("library_visual_scenes");
if (libVisualScenes==0)
if (libVisualScenes == 0)
return;
{
for(XMLElement* scene = libVisualScenes->FirstChildElement("visual_scene");
scene != NULL; scene = scene->NextSiblingElement("visual_scene"))
for (XMLElement* scene = libVisualScenes->FirstChildElement("visual_scene");
scene != NULL; scene = scene->NextSiblingElement("visual_scene"))
{
const char* sceneName = scene->Attribute("id");
allVisualScenes.insert(sceneName,scene);
allVisualScenes.insert(sceneName, scene);
}
}
@@ -477,7 +473,7 @@ void readVisualSceneInstanceGeometries(XMLDocument& doc, btHashMap<btHashString,
if (instanceSceneReference)
{
const char* instanceSceneUrl = instanceSceneReference->Attribute("url");
XMLElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl+1];//skip #
XMLElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl + 1]; //skip #
if (sceneInstancePtr)
{
scene = *sceneInstancePtr;
@@ -488,28 +484,26 @@ void readVisualSceneInstanceGeometries(XMLDocument& doc, btHashMap<btHashString,
if (scene)
{
for(XMLElement* node = scene->FirstChildElement("node");
node != NULL; node = node->NextSiblingElement("node"))
for (XMLElement* node = scene->FirstChildElement("node");
node != NULL; node = node->NextSiblingElement("node"))
{
btMatrix4x4 identity;
identity.setIdentity();
btVector3 identScaling(1,1,1);
readNodeHierarchy(node,name2Shape,visualShapeInstances, identity);
btVector3 identScaling(1, 1, 1);
readNodeHierarchy(node, name2Shape, visualShapeInstances, identity);
}
}
}
void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
{
///todo(erwincoumans) those up-axis transformations have been quickly coded without rigorous testing
XMLElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
if (unitMeter)
{
const char* meterText = unitMeter->Attribute("meter");
//printf("meterText=%s\n", meterText);
//printf("meterText=%s\n", meterText);
unitMeterScaling = atof(meterText);
}
@@ -518,13 +512,12 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
{
switch (clientUpAxis)
{
case 1:
{
std::string upAxisTxt = upAxisElem->GetText();
if (upAxisTxt == "X_UP")
{
btQuaternion x2y(btVector3(0,0,1),SIMD_HALF_PI);
btQuaternion x2y(btVector3(0, 0, 1), SIMD_HALF_PI);
tr.setRotation(x2y);
}
if (upAxisTxt == "Y_UP")
@@ -534,7 +527,7 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
}
if (upAxisTxt == "Z_UP")
{
btQuaternion z2y(btVector3(1,0,0),-SIMD_HALF_PI);
btQuaternion z2y(btVector3(1, 0, 0), -SIMD_HALF_PI);
tr.setRotation(z2y);
}
break;
@@ -544,12 +537,12 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
std::string upAxisTxt = upAxisElem->GetText();
if (upAxisTxt == "X_UP")
{
btQuaternion x2z(btVector3(0,1,0),-SIMD_HALF_PI);
btQuaternion x2z(btVector3(0, 1, 0), -SIMD_HALF_PI);
tr.setRotation(x2z);
}
if (upAxisTxt == "Y_UP")
{
btQuaternion y2z(btVector3(1,0,0),SIMD_HALF_PI);
btQuaternion y2z(btVector3(1, 0, 0), SIMD_HALF_PI);
tr.setRotation(y2z);
}
if (upAxisTxt == "Z_UP")
@@ -568,46 +561,42 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
}
}
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis)
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis)
{
// GLInstanceGraphicsShape* instance = 0;
// GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(MAX_VISUAL_SHAPES);
float extraScaling = 1;//0.01;
float extraScaling = 1; //0.01;
btHashMap<btHashString, int> name2ShapeIndex;
b3FileUtils f;
char filename[1024];
if (!f.findFile(relativeFileName,filename,1024))
if (!f.findFile(relativeFileName, filename, 1024))
{
b3Warning("File not found: %s\n", filename);
return;
}
XMLDocument doc;
if (doc.LoadFile(filename) != XML_SUCCESS)
return;
//We need units to be in meter, so apply a scaling using the asset/units meter
unitMeterScaling=1;
//We need units to be in meter, so apply a scaling using the asset/units meter
unitMeterScaling = 1;
upAxisTransform.setIdentity();
//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling,clientUpAxis);
getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling, clientUpAxis);
btMatrix4x4 ident;
ident.setIdentity();
readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);
}
#ifdef COMPARE_WITH_ASSIMP
#include <assimp/Importer.hpp>
@@ -615,127 +604,128 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
#include <assimp/postprocess.h>
#include <assimp/scene.h>
# include "assimp/ColladaLoader.h"
#include "assimp/ColladaLoader.h"
//# include "STLLoader.h"
# include "assimp/SortByPTypeProcess.h"
# include "assimp/LimitBoneWeightsProcess.h"
# include "assimp/TriangulateProcess.h"
# include "assimp/JoinVerticesProcess.h"
# include "assimp/RemoveVCProcess.h"
#include "assimp/SortByPTypeProcess.h"
#include "assimp/LimitBoneWeightsProcess.h"
#include "assimp/TriangulateProcess.h"
#include "assimp/JoinVerticesProcess.h"
#include "assimp/RemoveVCProcess.h"
namespace Assimp {
// ------------------------------------------------------------------------------------------------
void GetImporterInstanceList(std::vector< BaseImporter* >& out)
{
out.push_back( new ColladaLoader());
}
// ------------------------------------------------------------------------------------------------
void GetPostProcessingStepInstanceList(std::vector< BaseProcess* >& out)
{
out.push_back( new SortByPTypeProcess());
out.push_back( new LimitBoneWeightsProcess());
out.push_back( new TriangulateProcess());
out.push_back( new JoinVerticesProcess());
//out.push_back( new RemoveVCProcess());
}
namespace Assimp
{
// ------------------------------------------------------------------------------------------------
void GetImporterInstanceList(std::vector<BaseImporter*>& out)
{
out.push_back(new ColladaLoader());
}
// ------------------------------------------------------------------------------------------------
void GetPostProcessingStepInstanceList(std::vector<BaseProcess*>& out)
{
out.push_back(new SortByPTypeProcess());
out.push_back(new LimitBoneWeightsProcess());
out.push_back(new TriangulateProcess());
out.push_back(new JoinVerticesProcess());
//out.push_back( new RemoveVCProcess());
}
} // namespace Assimp
static void addMeshParts(const aiScene* scene, const aiNode* node, GLInstanceGraphicsShape* outverts, const aiMatrix4x4& parentTr)
{
aiMatrix4x4 const& nodeTrans(node->mTransformation);
aiMatrix4x4 trans;
trans = parentTr * nodeTrans;
for (size_t i = 0; i < node->mNumMeshes; ++i)
aiMatrix4x4 const& nodeTrans(node->mTransformation);
aiMatrix4x4 trans;
trans = parentTr * nodeTrans;
for (size_t i = 0; i < node->mNumMeshes; ++i)
{
aiMesh const* mesh = scene->mMeshes[node->mMeshes[i]];
size_t num_vertices = mesh->mNumVertices;
if (mesh->mPrimitiveTypes == aiPrimitiveType_TRIANGLE)
{
aiMesh const* mesh = scene->mMeshes[node->mMeshes[i]];
size_t num_vertices = mesh->mNumVertices;
if (mesh->mPrimitiveTypes==aiPrimitiveType_TRIANGLE)
int curVertexBase = outverts->m_vertices->size();
for (int v = 0; v < mesh->mNumVertices; v++)
{
int curVertexBase = outverts->m_vertices->size();
for (int v=0;v<mesh->mNumVertices;v++)
GLInstanceVertex vtx;
aiVector3D vWorld = trans * mesh->mVertices[v];
vtx.xyzw[0] = vWorld.x;
vtx.xyzw[1] = vWorld.y;
vtx.xyzw[2] = vWorld.z;
vtx.xyzw[3] = 1;
if (mesh->HasNormals())
{
GLInstanceVertex vtx;
aiVector3D vWorld = trans*mesh->mVertices[v];
vtx.xyzw[0] = vWorld.x;
vtx.xyzw[1] = vWorld.y;
vtx.xyzw[2] = vWorld.z;
vtx.xyzw[3] = 1;
if (mesh->HasNormals())
{
vtx.normal[0] = mesh->mNormals[v].x;
vtx.normal[1] = mesh->mNormals[v].y;
vtx.normal[2] = mesh->mNormals[v].z;
} else
{
vtx.normal[0] = 0;
vtx.normal[1] = 0;
vtx.normal[2] = 1;
}
if (mesh->HasTextureCoords(0))
{
vtx.uv[0] = mesh->mTextureCoords[0][v].x;
vtx.uv[1] = mesh->mTextureCoords[0][v].y;
} else
{
vtx.uv[0]=0.5f;
vtx.uv[1]=0.5f;
}
outverts->m_vertices->push_back(vtx);
vtx.normal[0] = mesh->mNormals[v].x;
vtx.normal[1] = mesh->mNormals[v].y;
vtx.normal[2] = mesh->mNormals[v].z;
}
for (int f=0;f<mesh->mNumFaces;f++)
else
{
b3Assert(mesh->mFaces[f].mNumIndices == 3);
int i0 = mesh->mFaces[f].mIndices[0];
int i1 = mesh->mFaces[f].mIndices[1];
int i2 = mesh->mFaces[f].mIndices[2];
outverts->m_indices->push_back(i0+curVertexBase);
outverts->m_indices->push_back(i1+curVertexBase);
outverts->m_indices->push_back(i2+curVertexBase);
vtx.normal[0] = 0;
vtx.normal[1] = 0;
vtx.normal[2] = 1;
}
if (mesh->HasTextureCoords(0))
{
vtx.uv[0] = mesh->mTextureCoords[0][v].x;
vtx.uv[1] = mesh->mTextureCoords[0][v].y;
}
else
{
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
}
outverts->m_vertices->push_back(vtx);
}
}
for (size_t i=0 ; i<node->mNumChildren ; ++i) {
addMeshParts(scene,node->mChildren[i], outverts, trans);
}
for (int f = 0; f < mesh->mNumFaces; f++)
{
b3Assert(mesh->mFaces[f].mNumIndices == 3);
int i0 = mesh->mFaces[f].mIndices[0];
int i1 = mesh->mFaces[f].mIndices[1];
int i2 = mesh->mFaces[f].mIndices[2];
outverts->m_indices->push_back(i0 + curVertexBase);
outverts->m_indices->push_back(i1 + curVertexBase);
outverts->m_indices->push_back(i2 + curVertexBase);
}
}
}
for (size_t i = 0; i < node->mNumChildren; ++i)
{
addMeshParts(scene, node->mChildren[i], outverts, trans);
}
}
void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,btTransform& upAxisTrans, float& unitMeterScaling)
void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTrans, float& unitMeterScaling)
{
upAxisTrans.setIdentity();
unitMeterScaling=1;
unitMeterScaling = 1;
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName,"rb");
FILE* file = fopen(relativeFileName, "rb");
if (file)
{
int size=0;
int size = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
}
else
{
if (size)
{
//printf("Open DAE file of %d bytes\n",size);
Assimp::Importer importer;
//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
// importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
// importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
aiScene const* scene = importer.ReadFile(relativeFileName,
aiProcess_JoinIdenticalVertices |
//aiProcess_RemoveComponent |
aiProcess_SortByPType |
aiProcess_Triangulate);
aiProcess_JoinIdenticalVertices |
//aiProcess_RemoveComponent |
aiProcess_SortByPType |
aiProcess_Triangulate);
if (scene)
{
shape = &visualShapes.expand();
@@ -749,16 +739,14 @@ void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArra
aiMatrix4x4 ident;
addMeshParts(scene, scene->mRootNode, shape, ident);
shape->m_numIndices = shape->m_indices->size();
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = visualShapes.size()-1;
instance.m_shapeIndex = visualShapes.size() - 1;
}
}
}
}
}
#endif //COMPARE_WITH_ASSIMP
#endif //COMPARE_WITH_ASSIMP

25
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
View File

@@ -15,7 +15,6 @@ subject to the following restrictions:
//original author: Erwin Coumans
*/
#ifndef LOAD_MESH_FROM_COLLADA_H
#define LOAD_MESH_FROM_COLLADA_H
@@ -24,22 +23,20 @@ subject to the following restrictions:
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "ColladaGraphicsInstance.h"
void LoadMeshFromCollada(const char* relativeFileName,
btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling,
btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling,
int clientUpAxis);
//#define COMPARE_WITH_ASSIMP
#ifdef COMPARE_WITH_ASSIMP
void LoadMeshFromColladaAssimp(const char* relativeFileName,
btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling
);
#endif //COMPARE_WITH_ASSIMP
void LoadMeshFromColladaAssimp(const char* relativeFileName,
btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling);
#endif //COMPARE_WITH_ASSIMP
#endif //LOAD_MESH_FROM_COLLADA_H
#endif //LOAD_MESH_FROM_COLLADA_H

125
examples/Importers/ImportColladaDemo/btMatrix4x4.h
View File

@@ -15,7 +15,6 @@ subject to the following restrictions:
//original author: Erwin Coumans
*/
#ifndef MATRIX4x4_H
#define MATRIX4x4_H
@@ -23,55 +22,56 @@ subject to the following restrictions:
#include "LinearMath/btQuaternion.h"
///This 4x4 matrix class is extremely limited, just created for the purpose of accumulating transform matrices in COLLADA .dae files
ATTRIBUTE_ALIGNED16(class) btMatrix4x4
ATTRIBUTE_ALIGNED16(class)
btMatrix4x4
{
btVector4 m_el[4];
public:
public:
btMatrix4x4()
{
}
btMatrix4x4(const btScalar& xx, const btScalar& xy, const btScalar& xz,const btScalar& xw,
const btScalar& yx, const btScalar& yy, const btScalar& yz,const btScalar& yw,
const btScalar& zx, const btScalar& zy, const btScalar& zz, const btScalar& zw,
const btScalar& wx, const btScalar& wy, const btScalar& wz, const btScalar& ww)
{
btMatrix4x4(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& xw,
const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& yw,
const btScalar& zx, const btScalar& zy, const btScalar& zz, const btScalar& zw,
const btScalar& wx, const btScalar& wy, const btScalar& wz, const btScalar& ww)
{
setValue(xx, xy, xz, xw,
yx, yy, yz, yw,
zx, zy, zz,zw,
wx, wy, wz,ww);
yx, yy, yz, yw,
zx, zy, zz, zw,
wx, wy, wz, ww);
}
~btMatrix4x4()
{
}
inline void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,const btScalar& xw,
const btScalar& yx, const btScalar& yy, const btScalar& yz,const btScalar& yw,
const btScalar& zx, const btScalar& zy, const btScalar& zz, const btScalar& zw,
const btScalar& wx, const btScalar& wy, const btScalar& wz, const btScalar& ww)
{
m_el[0].setValue(xx,xy,xz,xw);
m_el[1].setValue(yx,yy,yz,yw);
m_el[2].setValue(zx,zy,zz,zw);
m_el[3].setValue(wx,wy,wz,ww);
inline void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& xw,
const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& yw,
const btScalar& zx, const btScalar& zy, const btScalar& zz, const btScalar& zw,
const btScalar& wx, const btScalar& wy, const btScalar& wz, const btScalar& ww)
{
m_el[0].setValue(xx, xy, xz, xw);
m_el[1].setValue(yx, yy, yz, yw);
m_el[2].setValue(zx, zy, zz, zw);
m_el[3].setValue(wx, wy, wz, ww);
}
inline void setIdentity()
{
m_el[0].setValue(1,0,0,0);
m_el[1].setValue(0,1,0,0);
m_el[2].setValue(0,0,1,0);
m_el[3].setValue(0,0,0,1);
m_el[0].setValue(1, 0, 0, 0);
m_el[1].setValue(0, 1, 0, 0);
m_el[2].setValue(0, 0, 1, 0);
m_el[3].setValue(0, 0, 0, 1);
}
inline void setPureRotation(const btQuaternion& orn)
{
setIdentity();
btMatrix3x3 m3(orn);
for (int i=0;i<3;i++)
for (int i = 0; i < 3; i++)
{
for (int j=0;j<3;j++)
for (int j = 0; j < 3; j++)
{
m_el[i][j] = m3[i][j];
}
@@ -80,78 +80,73 @@ ATTRIBUTE_ALIGNED16(class) btMatrix4x4
inline void setPureScaling(const btVector3& scale)
{
m_el[0].setValue(scale[0],0,0,0);
m_el[1].setValue(0,scale[1],0,0);
m_el[2].setValue(0,0,scale[2],0);
m_el[3].setValue(0,0,0,1);
m_el[0].setValue(scale[0], 0, 0, 0);
m_el[1].setValue(0, scale[1], 0, 0);
m_el[2].setValue(0, 0, scale[2], 0);
m_el[3].setValue(0, 0, 0, 1);
}
inline void setPureTranslation(const btVector3& pos)
{
m_el[0].setValue(1,0,0,pos[0]);
m_el[1].setValue(0,1,0,pos[1]);
m_el[2].setValue(0,0,1,pos[2]);
m_el[3].setValue(0,0,0,1);
m_el[0].setValue(1, 0, 0, pos[0]);
m_el[1].setValue(0, 1, 0, pos[1]);
m_el[2].setValue(0, 0, 1, pos[2]);
m_el[3].setValue(0, 0, 0, 1);
}
SIMD_FORCE_INLINE const btVector4& operator[](int i) const
{
btFullAssert(0 <= i && i < 3);
return m_el[i];
return m_el[i];
}
SIMD_FORCE_INLINE btScalar tdotx(const btVector4& v) const
SIMD_FORCE_INLINE btScalar tdotx(const btVector4& v) const
{
return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z() + m_el[3].x()* v.w();
return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z() + m_el[3].x() * v.w();
}
SIMD_FORCE_INLINE btScalar tdoty(const btVector4& v) const
SIMD_FORCE_INLINE btScalar tdoty(const btVector4& v) const
{
return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z() + m_el[3].y() * v.w();
}
SIMD_FORCE_INLINE btScalar tdotz(const btVector4& v) const
SIMD_FORCE_INLINE btScalar tdotz(const btVector4& v) const
{
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z() + m_el[3].z() * v.w();
}
SIMD_FORCE_INLINE btScalar tdotw(const btVector4& v) const
SIMD_FORCE_INLINE btScalar tdotw(const btVector4& v) const
{
return m_el[0].w() * v.x() + m_el[1].w() * v.y() + m_el[2].w() * v.z() + m_el[3].w() * v.w();
}
SIMD_FORCE_INLINE btMatrix4x4 &
SIMD_FORCE_INLINE btMatrix4x4&
operator*=(const btMatrix4x4& m)
{
setValue(
m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),m.tdotw(m_el[0]),
m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),m.tdotw(m_el[1]),
m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]),m.tdotw(m_el[2]),
m.tdotx(m_el[3]), m.tdoty(m_el[3]), m.tdotz(m_el[3]),m.tdotw(m_el[3]));
m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), m.tdotw(m_el[0]),
m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), m.tdotw(m_el[1]),
m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]), m.tdotw(m_el[2]),
m.tdotx(m_el[3]), m.tdoty(m_el[3]), m.tdotz(m_el[3]), m.tdotw(m_el[3]));
return *this;
}
};
inline btScalar btDot4(const btVector4& v0, const btVector4& v1)
{
return v0.x()*v1.x()+v0.y()*v1.y()+v0.z()*v1.z()+v0.w()*v1.w();
return v0.x() * v1.x() + v0.y() * v1.y() + v0.z() * v1.z() + v0.w() * v1.w();
}
SIMD_FORCE_INLINE btVector3
operator*(const btMatrix4x4& m, const btVector3& v1)
SIMD_FORCE_INLINE btVector3
operator*(const btMatrix4x4& m, const btVector3& v1)
{
btVector4 v(v1[0],v1[1],v1[2],1);
return btVector3(btDot4(m[0],v), btDot4(m[1],v), btDot4(m[2],v));
btVector4 v(v1[0], v1[1], v1[2], 1);
return btVector3(btDot4(m[0], v), btDot4(m[1], v), btDot4(m[2], v));
}
SIMD_FORCE_INLINE btMatrix4x4
operator*(const btMatrix4x4& m1, btMatrix4x4& m2)
{
return btMatrix4x4(
m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),m2.tdotw(m1[0]),
m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),m2.tdotw(m1[1]),
m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]),m2.tdotw(m1[2]),
m2.tdotx(m1[3]), m2.tdoty(m1[3]), m2.tdotz(m1[3]),m2.tdotw(m1[3]));
}
SIMD_FORCE_INLINE btMatrix4x4
operator*(const btMatrix4x4& m1, btMatrix4x4& m2)
{
return btMatrix4x4(
m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]), m2.tdotw(m1[0]),
m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]), m2.tdotw(m1[1]),
m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]), m2.tdotw(m1[2]),
m2.tdotx(m1[3]), m2.tdoty(m1[3]), m2.tdotz(m1[3]), m2.tdotw(m1[3]));
}
#endif //MATRIX4x4_H
#endif //MATRIX4x4_H

1833
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

File diff suppressed because it is too large Load Diff

78
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
View File

@@ -4,13 +4,12 @@
#include "../ImportURDFDemo/URDFImporterInterface.h"
#include "../ImportURDFDemo/UrdfRenderingInterface.h"
struct MJCFErrorLogger
{
virtual ~MJCFErrorLogger() {}
virtual void reportError(const char* error)=0;
virtual void reportWarning(const char* warning)=0;
virtual void printMessage(const char* msg)=0;
virtual void reportError(const char* error) = 0;
virtual void reportWarning(const char* warning) = 0;
virtual void printMessage(const char* msg) = 0;
};
struct MJCFURDFTexture
@@ -30,27 +29,26 @@ class BulletMJCFImporter : public URDFImporterInterface
public:
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags);
virtual ~BulletMJCFImporter();
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
virtual bool loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase = false);
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
{
return false;
}
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; }
virtual const char* getPathPrefix();
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const;
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const;
virtual const char* getPathPrefix();
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const;
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const;
virtual std::string getBodyName() const;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
@@ -58,40 +56,38 @@ public:
bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
//optional method to get collision group (type) and mask (affinity)
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const ;
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
virtual std::string getJointName(int linkIndex) const;
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo) const;
virtual std::string getJointName(int linkIndex) const;
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const ;
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
virtual int getNumAllocatedCollisionShapes() const;
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
virtual void activateModel(int modelIndex);
virtual int getNumAllocatedMeshInterfaces() const;
virtual btStridingMeshInterface* getAllocatedMeshInterface(int index);
};
#endif //BULLET_MJCF_IMPORTER_H
#endif //BULLET_MJCF_IMPORTER_H

208
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@@ -19,86 +19,79 @@
class ImportMJCFSetup : public CommonMultiBodyBase
{
char m_fileName[1024];
char m_fileName[1024];
struct ImportMJCFInternalData* m_data;
struct ImportMJCFInternalData* m_data;
bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory;
btAlignedObjectArray<std::string*> m_nameMemory;
btScalar m_grav;
int m_upAxis;
public:
ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportMJCFSetup();
ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportMJCFSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void setFileName(const char* mjcfFileName);
void setFileName(const char* mjcfFileName);
virtual void resetCamera()
{
float dist = 3.5;
float pitch = -28;
float yaw = -136;
float targetPos[3]={0.47,0,-0.64};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {0.47, 0, -0.64};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
static btAlignedObjectArray<std::string> gMCFJFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportMJCFInternalData
{
ImportMJCFInternalData()
:m_numMotors(0),
m_mb(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
ImportMJCFInternalData()
: m_numMotors(0),
m_mb(0)
{
for (int i = 0; i < MAX_NUM_MOTORS; i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
}
btScalar m_motorTargetPositions[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
btMultiBody* m_mb;
btRigidBody* m_rb;
}
btScalar m_motorTargetPositions[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
int m_numMotors;
btMultiBody* m_mb;
btRigidBody* m_rb;
};
ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper),
m_grav(-10),
m_upAxis(2)
: CommonMultiBodyBase(helper),
m_grav(-10),
m_upAxis(2)
{
m_data = new ImportMJCFInternalData;
m_useMultiBody = true;
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
}
else
{
gMCFJFileNameArray.clear();
//load additional MJCF file names from file
FILE* f = fopen("mjcf_files.txt","r");
FILE* f = fopen("mjcf_files.txt", "r");
if (f)
{
int result;
@@ -106,18 +99,18 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("mjcf_files.txt entry %s",fileName);
if (result==1)
result = fscanf(f, "%s", fileName);
b3Printf("mjcf_files.txt entry %s", fileName);
if (result == 1)
{
gMCFJFileNameArray.push_back(fileName);
}
} while (result==1);
} while (result == 1);
fclose(f);
}
if (gMCFJFileNameArray.size()==0)
if (gMCFJFileNameArray.size() == 0)
{
gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
@@ -126,7 +119,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
gMCFJFileNameArray.push_back("mjcf/ant.xml");
gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml");
gMCFJFileNameArray.push_back("mjcf/cylinder.xml");
gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoX.xml");
gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoY.xml");
@@ -136,7 +129,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
gMCFJFileNameArray.push_back("mjcf/capsule_fromtoX.xml");
gMCFJFileNameArray.push_back("mjcf/capsule_fromtoY.xml");
gMCFJFileNameArray.push_back("mjcf/capsule_fromtoZ.xml");
gMCFJFileNameArray.push_back("mjcf/hopper.xml");
gMCFJFileNameArray.push_back("mjcf/swimmer.xml");
gMCFJFileNameArray.push_back("mjcf/reacher.xml");
@@ -144,32 +137,29 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
int numFileNames = gMCFJFileNameArray.size();
if (count>=numFileNames)
if (count >= numFileNames)
{
count=0;
count = 0;
}
sprintf(m_fileName,"%s",gMCFJFileNameArray[count++].c_str());
sprintf(m_fileName, "%s", gMCFJFileNameArray[count++].c_str());
}
}
ImportMJCFSetup::~ImportMJCFSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
for (int i = 0; i < m_nameMemory.size(); i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
delete m_data;
}
void ImportMJCFSetup::setFileName(const char* mjcfFileName)
{
memcpy(m_fileName,mjcfFileName,strlen(mjcfFileName)+1);
memcpy(m_fileName, mjcfFileName, strlen(mjcfFileName) + 1);
}
struct MyMJCFLogger : public MJCFErrorLogger
{
virtual void reportError(const char* error)
@@ -186,27 +176,20 @@ struct MyMJCFLogger : public MJCFErrorLogger
}
};
void ImportMJCFSetup::initPhysics()
{
m_guiHelper->setUpAxis(m_upAxis);
createEmptyDynamicsWorld();
//MuJoCo uses a slightly different collision filter mode, use the FILTER_GROUPAMASKB_OR_GROUPBMASKA2
//@todo also use the modified collision filter for raycast and other collision related queries
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
//m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
if (m_guiHelper->getParameterInterface())
{
@@ -216,16 +199,16 @@ void ImportMJCFSetup::initPhysics()
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
int flags=0;
BulletMJCFImporter importer(m_guiHelper, 0,flags);
int flags = 0;
BulletMJCFImporter importer(m_guiHelper, 0, flags);
MyMJCFLogger logger;
bool result = importer.loadMJCF(m_fileName,&logger);
bool result = importer.loadMJCF(m_fileName, &logger);
if (result)
{
btTransform rootTrans;
rootTrans.setIdentity();
for (int m =0; m<importer.getNumModels();m++)
for (int m = 0; m < importer.getNumModels(); m++)
{
importer.activateModel(m);
@@ -235,7 +218,6 @@ void ImportMJCFSetup::initPhysics()
btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
//int rootLinkIndex = importer.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
@@ -244,34 +226,34 @@ void ImportMJCFSetup::initPhysics()
rootTrans.setIdentity();
importer.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
ConvertURDF2Bullet(importer, creation, rootTrans, m_dynamicsWorld, m_useMultiBody, importer.getPathPrefix(), CUF_USE_MJCF);
mb = creation.getBulletMultiBody();
if (mb)
{
std::string* name =
new std::string(importer.getLinkName(
importer.getRootLinkIndex()));
m_nameMemory.push_back(name);
new std::string(importer.getLinkName(
importer.getRootLinkIndex()));
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(), name->c_str());
#endif //TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(importer.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(importer.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(), jointName->c_str());
s->registerNameForPointer(linkName->c_str(), linkName->c_str());
#endif //TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLinkCollider(i)->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
@@ -279,83 +261,76 @@ void ImportMJCFSetup::initPhysics()
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
m_data->m_mb = mb;
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
if (m_data->m_numMotors < MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q ", jointName->c_str());
sprintf(motorName, "%s q ", jointName->c_str());
btScalar* motorPos = &m_data->m_motorTargetPositions[m_data->m_numMotors];
*motorPos = 0.f;
SliderParams slider(motorName,motorPos);
slider.m_minVal=-4;
slider.m_maxVal=4;
SliderParams slider(motorName, motorPos);
slider.m_minVal = -4;
slider.m_maxVal = 4;
slider.m_clampToIntegers = false;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 5.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
motor->setErp(0.1);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_data->m_jointMotors[m_data->m_numMotors] = motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
} else
}
else
{
// not multibody
if (1)
{
//create motors for each generic joint
int num6Dof = creation.getNum6DofConstraints();
for (int i=0;i<num6Dof;i++)
for (int i = 0; i < num6Dof; i++)
{
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr())
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType ==URDFContinuousJoint))
if ((jointInfo->m_urdfJointType == URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType == URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType == URDFContinuousJoint))
{
int urdfLinkIndex = jointInfo->m_urdfIndex;
std::string jointName = importer.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
sprintf(motorName, "%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetPositions[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
SliderParams slider(motorName, motorVel);
slider.m_minVal = -4;
slider.m_maxVal = 4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
m_data->m_generic6DofJointMotors[m_data->m_numMotors] = c;
bool motorOn = true;
c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
c->enableMotor(jointInfo->m_jointAxisIndex, motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex, 10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex, 0);
m_data->m_numMotors++;
}
}
}
}
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
}
void ImportMJCFSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
@@ -364,8 +339,8 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
gravity[m_upAxis] = m_grav;
m_dynamicsWorld->setGravity(gravity);
for (int i=0;i<m_data->m_numMotors;i++)
{
for (int i = 0; i < m_data->m_numMotors; i++)
{
if (m_data->m_jointMotors[i])
{
btScalar pos = m_data->m_motorTargetPositions[i];
@@ -382,17 +357,16 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
if (m_data->m_generic6DofJointMotors[i])
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetPositions[i]);
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetPositions[i]);
}
}
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
}
}
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options)
{
return new ImportMJCFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
return new ImportMJCFSetup(options.m_guiHelper, options.m_option, options.m_fileName);
}

6
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h
View File

@@ -1,8 +1,6 @@
#ifndef IMPORT_MJCF_SETUP_H
#define IMPORT_MJCF_SETUP_H
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_MJCF_SETUP_H
#endif //IMPORT_MJCF_SETUP_H

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

@@ -1,7 +1,7 @@
#include "b3ImportMeshUtility.h"
#include <vector>
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "LinearMath/btVector3.h"
#include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h"
#include "../../Utils/b3ResourcePath.h"
@@ -10,7 +10,6 @@
#include "../ImportObjDemo/LoadMeshFromObj.h"
#include "Bullet3Common/b3HashMap.h"
struct CachedTextureResult
{
std::string m_textureName;
@@ -19,14 +18,11 @@ struct CachedTextureResult
int m_height;
unsigned char* m_pixels;
CachedTextureResult()
:m_width(0),
m_height(0),
m_pixels(0)
: m_width(0),
m_height(0),
m_pixels(0)
{
}
};
static b3HashMap<b3HashString, CachedTextureResult> gCachedTextureResults;
@@ -37,7 +33,7 @@ struct CachedTextureManager
}
virtual ~CachedTextureManager()
{
for (int i=0;i<gCachedTextureResults.size();i++)
for (int i = 0; i < gCachedTextureResults.size(); i++)
{
CachedTextureResult* res = gCachedTextureResults.getAtIndex(i);
if (res)
@@ -49,9 +45,6 @@ struct CachedTextureManager
};
static CachedTextureManager sTexCacheMgr;
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
{
B3_PROFILE("loadAndRegisterMeshFromFileInternal");
@@ -62,20 +55,20 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
meshData.m_isCached = false;
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
char pathPrefix[1024];
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
char pathPrefix[1024];
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
btVector3 shift(0, 0, 0);
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
btVector3 shift(0,0,0);
std::vector<tinyobj::shape_t> shapes;
{
B3_PROFILE("tinyobj::LoadObj");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix);
//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
}
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
{
B3_PROFILE("Load Texture");
@@ -86,19 +79,18 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
const tinyobj::shape_t& shape = shapes[i];
if (shape.material.diffuse_texname.length() > 0)
{
int width, height, n;
const char* filename = shape.material.diffuse_texname.c_str();
unsigned char* image = 0;
const char* prefix[] = { pathPrefix, "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
const char* prefix[] = {pathPrefix, "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numprefix = sizeof(prefix) / sizeof(const char*);
for (int i = 0; !image && i < numprefix; i++)
{
char relativeFileName[1024];
sprintf(relativeFileName, "%s%s", prefix[i], filename);
char relativeFileName2[1024];
char relativeFileName2[1024];
if (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024))
{
if (b3IsFileCachingEnabled())
@@ -116,10 +108,10 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
}
}
if (image==0)
if (image == 0)
{
image = stbi_load(relativeFileName, &width, &height, &n, 3);
meshData.m_textureImage1 = image;
if (image)
@@ -135,17 +127,16 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
result.m_height = height;
result.m_pixels = image;
meshData.m_isCached = true;
gCachedTextureResults.insert(relativeFileName,result);
gCachedTextureResults.insert(relativeFileName, result);
}
}
else
{
b3Warning("Unsupported texture image format [%s]\n", relativeFileName);
break;
}
}
}
else
{
@@ -154,18 +145,14 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
}
}
}
}
meshData.m_gfxShape = gfxShape;
return true;
}
else
{
b3Warning("Cannot find %s\n", fileName.c_str());
}
else
{
b3Warning("Cannot find %s\n", fileName.c_str());
}
return false;
}

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

@@ -7,7 +7,7 @@ struct b3ImportMeshData
{
struct GLInstanceGraphicsShape* m_gfxShape;
unsigned char* m_textureImage1;//in 3 component 8-bit RGB data
unsigned char* m_textureImage1; //in 3 component 8-bit RGB data
bool m_isCached;
int m_textureWidth;
int m_textureHeight;
@@ -16,11 +16,7 @@ struct b3ImportMeshData
class b3ImportMeshUtility
{
public:
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
};
#endif //B3_IMPORT_MESH_UTILITY_H
#endif //B3_IMPORT_MESH_UTILITY_H

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

@@ -1,7 +1,7 @@
#include "ImportObjExample.h"
#include <vector>
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include"Wavefront/tiny_obj_loader.h"
#include "Wavefront/tiny_obj_loader.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "btBulletDynamicsCommon.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
@@ -16,14 +16,12 @@
class ImportObjSetup : public CommonRigidBodyBase
{
std::string m_fileName;
std::string m_fileName;
public:
ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportObjSetup();
ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportObjSetup();
virtual void initPhysics();
virtual void resetCamera()
@@ -31,48 +29,45 @@ public:
float dist = 18;
float pitch = -46;
float yaw = 120;
float targetPos[3]={-2,-2,-2};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {-2, -2, -2};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName)
:CommonRigidBodyBase(helper)
: CommonRigidBodyBase(helper)
{
if (fileName)
{
m_fileName = fileName;
} else
{
m_fileName = "cube.obj";//"sponza_closed.obj";//sphere8.obj";
}
if (fileName)
{
m_fileName = fileName;
}
else
{
m_fileName = "cube.obj"; //"sponza_closed.obj";//sphere8.obj";
}
}
ImportObjSetup::~ImportObjSetup()
{
}
int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer)
{
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
{
int textureIndex = -1;
if (meshData.m_textureImage1)
{
textureIndex = renderer->registerTexture(meshData.m_textureImage1,meshData.m_textureWidth,meshData.m_textureHeight);
textureIndex = renderer->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
}
shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,
&meshData.m_gfxShape->m_indices->at(0),
shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,
&meshData.m_gfxShape->m_indices->at(0),
meshData.m_gfxShape->m_numIndices,
B3_GL_TRIANGLES,
textureIndex);
@@ -85,8 +80,6 @@ int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterf
return shapeId;
}
void ImportObjSetup::initPhysics()
{
m_guiHelper->setUpAxis(2);
@@ -94,26 +87,24 @@ void ImportObjSetup::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
btTransform trans;
trans.setIdentity();
trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
trans.setIdentity();
trans.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI));
btVector3 position = trans.getOrigin();
btQuaternion orn = trans.getRotation();
btVector3 scaling(1,1,1);
btVector3 color(1,1,1);
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
if (shapeId>=0)
{
//int id =
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
}
btVector3 scaling(1, 1, 1);
btVector3 color(1, 1, 1);
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
if (shapeId >= 0)
{
//int id =
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, position, orn, color, scaling);
}
}
CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options)
{
return new ImportObjSetup(options.m_guiHelper, options.m_fileName);
}
CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options)
{
return new ImportObjSetup(options.m_guiHelper, options.m_fileName);
}

5
examples/Importers/ImportObjDemo/ImportObjExample.h
View File

@@ -1,7 +1,6 @@
#ifndef IMPORT_OBJ_EXAMPLE_H
#define IMPORT_OBJ_EXAMPLE_H
class CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_OBJ_EXAMPLE_H
#endif //IMPORT_OBJ_EXAMPLE_H

18
examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
View File

@@ -1,7 +1,7 @@
#include "LoadMeshFromObj.h"
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
#include <vector>
@@ -23,18 +23,17 @@ int b3IsFileCachingEnabled()
}
void b3EnableFileCaching(int enable)
{
gEnableFileCaching = enable;
if (enable==0)
gEnableFileCaching = enable;
if (enable == 0)
{
gCachedObjResults.clear();
}
}
std::string LoadFromCachedOrFromObj(
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath)
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath)
{
CachedObjResult* resultPtr = gCachedObjResults[filename];
if (resultPtr)
@@ -50,19 +49,18 @@ std::string LoadFromCachedOrFromObj(
result.m_shapes = shapes;
if (gEnableFileCaching)
{
gCachedObjResults.insert(filename,result);
gCachedObjResults.insert(filename, result);
}
return err;
}
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath)
{
B3_PROFILE("LoadMeshFromObj");
std::vector<tinyobj::shape_t> shapes;
{
B3_PROFILE("tinyobj::LoadObj2");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath);
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath);
}
{

12
examples/Importers/ImportObjDemo/LoadMeshFromObj.h
View File

@@ -1,20 +1,18 @@
#ifndef LOAD_MESH_FROM_OBJ_H
#define LOAD_MESH_FROM_OBJ_H
struct GLInstanceGraphicsShape;
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
int b3IsFileCachingEnabled();
void b3EnableFileCaching(int enable);
std::string LoadFromCachedOrFromObj(
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath);
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath);
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath);
#endif //LOAD_MESH_FROM_OBJ_H
#endif //LOAD_MESH_FROM_OBJ_H

217
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
View File

@@ -11,188 +11,181 @@
GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading)
{
b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
{
// int numVertices = obj->vertexCount;
// int numIndices = 0;
b3AlignedObjectArray<int>* indicesPtr = new b3AlignedObjectArray<int>;
for (int s=0;s<(int)shapes.size();s++)
for (int s = 0; s < (int)shapes.size(); s++)
{
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f=0;f<faceCount;f+=3)
for (int f = 0; f < faceCount; f += 3)
{
//btVector3 normal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
if (1)
{
btVector3 normal(0,1,0);
btVector3 normal(0, 1, 0);
int vtxBaseIndex = vertices->size();
if (f<0 && f>=int(shape.mesh.indices.size()))
if (f < 0 && f >= int(shape.mesh.indices.size()))
{
continue;
}
GLInstanceVertex vtx0;
vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0];
vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f]*3+1];
vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f]*3+2];
vtx0.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size() )
GLInstanceVertex vtx0;
vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 0];
vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 1];
vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 2];
vtx0.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
{
int uv0Index = shape.mesh.indices[f]*2+0;
int uv1Index = shape.mesh.indices[f]*2+1;
if (uv0Index>=0 && uv1Index>=0 && (uv0Index < int(shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())))
int uv0Index = shape.mesh.indices[f] * 2 + 0;
int uv1Index = shape.mesh.indices[f] * 2 + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())))
{
vtx0.uv[0] = shape.mesh.texcoords[uv0Index];
vtx0.uv[1] = shape.mesh.texcoords[uv1Index];
} else
}
else
{
// b3Warning("obj texture coordinate out-of-range!");
// b3Warning("obj texture coordinate out-of-range!");
vtx0.uv[0] = 0;
vtx0.uv[1] = 0;
}
} else
}
else
{
vtx0.uv[0] = 0.5;
vtx0.uv[1] = 0.5;
}
GLInstanceVertex vtx1;
vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f+1]*3+0];
vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f+1]*3+1];
vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f+1]*3+2];
vtx1.xyzw[3]= 0.f;
vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0];
vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1];
vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2];
vtx1.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
{
int uv0Index = shape.mesh.indices[f+1]*2+0;
int uv1Index = shape.mesh.indices[f+1]*2+1;
if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
int uv0Index = shape.mesh.indices[f + 1] * 2 + 0;
int uv1Index = shape.mesh.indices[f + 1] * 2 + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
{
vtx1.uv[0] = shape.mesh.texcoords[uv0Index];
vtx1.uv[1] = shape.mesh.texcoords[uv1Index];
} else
}
else
{
// b3Warning("obj texture coordinate out-of-range!");
// b3Warning("obj texture coordinate out-of-range!");
vtx1.uv[0] = 0;
vtx1.uv[1] = 0;
}
} else
}
else
{
vtx1.uv[0] = 0.5f;
vtx1.uv[1] = 0.5f;
}
GLInstanceVertex vtx2;
vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f+2]*3+0];
vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f+2]*3+1];
vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f+2]*3+2];
vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0];
vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1];
vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2];
vtx2.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
{
int uv0Index = shape.mesh.indices[f+2]*2+0;
int uv1Index = shape.mesh.indices[f+2]*2+1;
if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
int uv0Index = shape.mesh.indices[f + 2] * 2 + 0;
int uv1Index = shape.mesh.indices[f + 2] * 2 + 1;
if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
{
vtx2.uv[0] = shape.mesh.texcoords[uv0Index];
vtx2.uv[1] = shape.mesh.texcoords[uv1Index];
} else
}
else
{
b3Warning("obj texture coordinate out-of-range!");
vtx2.uv[0] = 0;
vtx2.uv[1] = 0;
}
} else
}
else
{
vtx2.uv[0] = 0.5;
vtx2.uv[1] = 0.5;
}
btVector3 v0(vtx0.xyzw[0],vtx0.xyzw[1],vtx0.xyzw[2]);
btVector3 v1(vtx1.xyzw[0],vtx1.xyzw[1],vtx1.xyzw[2]);
btVector3 v2(vtx2.xyzw[0],vtx2.xyzw[1],vtx2.xyzw[2]);
btVector3 v0(vtx0.xyzw[0], vtx0.xyzw[1], vtx0.xyzw[2]);
btVector3 v1(vtx1.xyzw[0], vtx1.xyzw[1], vtx1.xyzw[2]);
btVector3 v2(vtx2.xyzw[0], vtx2.xyzw[1], vtx2.xyzw[2]);
unsigned int maxIndex = 0;
maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+0);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+1);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+2);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+0);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+1);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+2);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+0);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+1);
maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+2);
bool hasNormals = (shape.mesh.normals.size() && maxIndex<shape.mesh.normals.size() );
maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 0);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 1);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 2);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 0);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 1);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 2);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 0);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 1);
maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 2);
bool hasNormals = (shape.mesh.normals.size() && maxIndex < shape.mesh.normals.size());
if (flatShading || !hasNormals)
{
normal = (v1-v0).cross(v2-v0);
btScalar len2 = normal.length2();
//skip degenerate triangles
if (len2 > SIMD_EPSILON)
{
normal.normalize();
} else
{
normal.setValue(0,0,0);
}
vtx0.normal[0] = normal[0];
vtx0.normal[1] = normal[1];
vtx0.normal[2] = normal[2];
vtx1.normal[0] = normal[0];
vtx1.normal[1] = normal[1];
vtx1.normal[2] = normal[2];
vtx2.normal[0] = normal[0];
vtx2.normal[1] = normal[1];
vtx2.normal[2] = normal[2];
} else
{
vtx0.normal[0] = shape.mesh.normals[shape.mesh.indices[f]*3+0];
vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f]*3+1];
vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f]*3+2]; //shape.mesh.indices[f+1]*3+0
vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f+1]*3+0];
vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f+1]*3+1];
vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f+1]*3+2];
vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f+2]*3+0];
vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f+2]*3+1];
vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f+2]*3+2];
}
vertices->push_back(vtx0);
vertices->push_back(vtx1);
vertices->push_back(vtx2);
indicesPtr->push_back(vtxBaseIndex);
indicesPtr->push_back(vtxBaseIndex+1);
indicesPtr->push_back(vtxBaseIndex+2);
{
normal = (v1 - v0).cross(v2 - v0);
btScalar len2 = normal.length2();
//skip degenerate triangles
if (len2 > SIMD_EPSILON)
{
normal.normalize();
}
else
{
normal.setValue(0, 0, 0);
}
vtx0.normal[0] = normal[0];
vtx0.normal[1] = normal[1];
vtx0.normal[2] = normal[2];
vtx1.normal[0] = normal[0];
vtx1.normal[1] = normal[1];
vtx1.normal[2] = normal[2];
vtx2.normal[0] = normal[0];
vtx2.normal[1] = normal[1];
vtx2.normal[2] = normal[2];
}
else
{
vtx0.normal[0] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 0];
vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 1];
vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 2]; //shape.mesh.indices[f+1]*3+0
vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 0];
vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 1];
vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 2];
vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 0];
vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 1];
vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 2];
}
vertices->push_back(vtx0);
vertices->push_back(vtx1);
vertices->push_back(vtx2);
indicesPtr->push_back(vtxBaseIndex);
indicesPtr->push_back(vtxBaseIndex + 1);
indicesPtr->push_back(vtxBaseIndex + 2);
}
}
}
GLInstanceGraphicsShape* gfxShape = new GLInstanceGraphicsShape;
gfxShape->m_vertices = vertices;
gfxShape->m_numvertices = vertices->size();
gfxShape->m_indices = indicesPtr;
gfxShape->m_numIndices = indicesPtr->size();
for (int i=0;i<4;i++)
gfxShape->m_scaling[i] = 1;//bake the scaling into the vertices
for (int i = 0; i < 4; i++)
gfxShape->m_scaling[i] = 1; //bake the scaling into the vertices
return gfxShape;
}
}

6
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
View File

@@ -1,9 +1,9 @@
#ifndef WAVEFRONT2GRAPHICS_H
#define WAVEFRONT2GRAPHICS_H
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include <vector>
struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading=false);
struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading = false);
#endif //WAVEFRONT2GRAPHICS_H
#endif //WAVEFRONT2GRAPHICS_H

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

@@ -2,7 +2,6 @@
#include "ImportSDFSetup.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "Bullet3Common/b3FileUtils.h"
@@ -13,104 +12,92 @@
#include "../ImportURDFDemo/BulletUrdfImporter.h"
#include "../ImportURDFDemo/URDF2Bullet.h"
//#include "urdf_samples.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
class ImportSDFSetup : public CommonMultiBodyBase
{
char m_fileName[1024];
char m_fileName[1024];
struct ImportSDFInternalData* m_data;
struct ImportSDFInternalData* m_data;
bool m_useMultiBody;
//todo(erwincoumans) we need a name memory for each model
btAlignedObjectArray<std::string* > m_nameMemory;
//todo(erwincoumans) we need a name memory for each model
btAlignedObjectArray<std::string*> m_nameMemory;
public:
ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportSDFSetup();
ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportSDFSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void setFileName(const char* urdfFileName);
void setFileName(const char* urdfFileName);
virtual void resetCamera()
{
float dist = 3.5;
float pitch = -28;
float yaw = -136;
float targetPos[3]={0.47,0,-0.64};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {0.47, 0, -0.64};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
static btAlignedObjectArray<std::string> gFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportSDFInternalData
{
ImportSDFInternalData()
:m_numMotors(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
ImportSDFInternalData()
: m_numMotors(0)
{
for (int i = 0; i < MAX_NUM_MOTORS; i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
int m_numMotors;
};
ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper)
: CommonMultiBodyBase(helper)
{
m_data = new ImportSDFInternalData;
(void)option;
// if (option==1)
// {
m_useMultiBody = true;
//
// } else
// {
// m_useMultiBody = false;
// }
// if (option==1)
// {
m_useMultiBody = true;
//
// } else
// {
// m_useMultiBody = false;
// }
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
}
else
{
gFileNameArray.clear();
//load additional urdf file names from file
FILE* f = fopen("sdf_files.txt","r");
FILE* f = fopen("sdf_files.txt", "r");
if (f)
{
int result;
@@ -118,79 +105,66 @@ ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, co
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("sdf_files.txt entry %s",fileName);
if (result==1)
result = fscanf(f, "%s", fileName);
b3Printf("sdf_files.txt entry %s", fileName);
if (result == 1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
} while (result == 1);
fclose(f);
}
if (gFileNameArray.size()==0)
if (gFileNameArray.size() == 0)
{
gFileNameArray.push_back("two_cubes.sdf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
if (count >= numFileNames)
{
count=0;
count = 0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
sprintf(m_fileName, "%s", gFileNameArray[count++].c_str());
}
}
ImportSDFSetup::~ImportSDFSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
for (int i = 0; i < m_nameMemory.size(); i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
delete m_data;
}
void ImportSDFSetup::setFileName(const char* urdfFileName)
{
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
memcpy(m_fileName, urdfFileName, strlen(urdfFileName) + 1);
}
void ImportSDFSetup::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8;
btVector3 gravity(0, 0, 0);
gravity[upAxis] = -9.8;
m_dynamicsWorld->setGravity(gravity);
BulletURDFImporter u2b(m_guiHelper, 0,1,0);
bool loadOk = u2b.loadSDF(m_fileName);
BulletURDFImporter u2b(m_guiHelper, 0, 1, 0);
bool loadOk = u2b.loadSDF(m_fileName);
if (loadOk)
{
@@ -198,123 +172,112 @@ void ImportSDFSetup::initPhysics()
//u2b.printTree();
btTransform rootTrans;
rootTrans.setIdentity();
for (int m =0; m<u2b.getNumModels();m++)
btTransform rootTrans;
rootTrans.setIdentity();
for (int m = 0; m < u2b.getNumModels(); m++)
{
u2b.activateModel(m);
u2b.activateModel(m);
btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
//int rootLinkIndex = u2b.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF);
u2b.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(u2b, creation, rootTrans, m_dynamicsWorld, m_useMultiBody, u2b.getPathPrefix(), CUF_USE_SDF);
mb = creation.getBulletMultiBody();
if (m_useMultiBody && mb )
if (m_useMultiBody && mb)
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
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
s->registerNameForPointer(name->c_str(), name->c_str());
#endif //TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(), jointName->c_str());
s->registerNameForPointer(linkName->c_str(), linkName->c_str());
#endif //TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
if (m_data->m_numMotors < MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
sprintf(motorName, "%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
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);
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_data->m_jointMotors[m_data->m_numMotors] = motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
}
}
}
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}
for (int i = 0; i < m_dynamicsWorld->getNumMultiBodyConstraints(); i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}
bool createGround=true;
bool createGround = true;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btVector3 groundHalfExtents(20, 20, 20);
groundHalfExtents[upAxis] = 1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2.5;
btTransform start;
start.setIdentity();
btVector3 groundOrigin(0, 0, 0);
groundOrigin[upAxis] = -2.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btRigidBody* body = createRigidBody(0, start, box);
//m_dynamicsWorld->removeRigidBody(body);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5,0.5,0.5);
m_guiHelper->createRigidBodyGraphicsObject(body,color);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5, 0.5, 0.5);
m_guiHelper->createRigidBodyGraphicsObject(body, color);
}
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
m_dynamicsWorld->stepSimulation(1. / 240., 0); // 1., 10, 1. / 240.);
}
}
void ImportSDFSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
for (int i=0;i<m_data->m_numMotors;i++)
{
for (int i = 0; i < m_data->m_numMotors; i++)
{
if (m_data->m_jointMotors[i])
{
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
@@ -322,18 +285,17 @@ void ImportSDFSetup::stepSimulation(float deltaTime)
if (m_data->m_generic6DofJointMotors[i])
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetVelocities[i]);
//jointInfo->
}
}
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
}
}
class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options)
{
return new ImportSDFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
return new ImportSDFSetup(options.m_guiHelper, options.m_option, options.m_fileName);
}

6
examples/Importers/ImportSDFDemo/ImportSDFSetup.h
View File

@@ -1,8 +1,6 @@
#ifndef IMPORT_SDF_SETUP_H
#define IMPORT_SDF_SETUP_H
class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_SDF_SETUP_H
#endif //IMPORT_SDF_SETUP_H

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

@@ -8,54 +8,45 @@
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../../Utils/b3ResourcePath.h"
class ImportSTLSetup : public CommonRigidBodyBase
{
const char* m_fileName;
btVector3 m_scaling;
public:
ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportSTLSetup();
ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportSTLSetup();
virtual void initPhysics();
virtual void resetCamera()
{
float dist = 3.5;
float pitch = -28;
float yaw = -136;
float targetPos[3]={0.47,0,-0.64};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {0.47, 0, -0.64};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
:CommonRigidBodyBase(helper),
m_scaling(btVector3(10,10,10))
: CommonRigidBodyBase(helper),
m_scaling(btVector3(10, 10, 10))
{
if (fileName)
{
m_fileName = fileName;
m_scaling = btVector3(0.01,0.01,0.01);
} else
m_scaling = btVector3(0.01, 0.01, 0.01);
}
else
{
m_fileName = "l_finger_tip.stl";
}
}
ImportSTLSetup::~ImportSTLSetup()
{
}
void ImportSTLSetup::initPhysics()
{
m_guiHelper->setUpAxis(2);
@@ -63,41 +54,35 @@ void ImportSTLSetup::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
{
b3Warning("Cannot find file %s\n", m_fileName);
return;
}
btVector3 shift(0,0,0);
// int index=10;
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
{
b3Warning("Cannot find file %s\n", m_fileName);
return;
}
btVector3 shift(0, 0, 0);
// int index=10;
{
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
btTransform trans;
trans.setIdentity();
trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
trans.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI));
btVector3 position = trans.getOrigin();
btQuaternion orn = trans.getRotation();
btVector3 color(0,0,1);
int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling);
btVector3 color(0, 0, 1);
int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, position, orn, color, m_scaling);
}
}
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options)
{
return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
}

4
examples/Importers/ImportSTLDemo/ImportSTLSetup.h
View File

@@ -1,6 +1,6 @@
#ifndef IMPORT_STL_SETUP_H
#define IMPORT_STL_SETUP_H
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_OBJ_SETUP_H
#endif //IMPORT_OBJ_SETUP_H

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

@@ -3,7 +3,7 @@
#define LOAD_MESH_FROM_STL_H
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
struct MySTLTriangle
@@ -17,46 +17,47 @@ struct MySTLTriangle
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
{
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName,"rb");
FILE* file = fopen(relativeFileName, "rb");
if (file)
{
int size=0;
int size = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
}
else
{
if (size)
{
//b3Warning("Open STL file of %d bytes\n",size);
char* memoryBuffer = new char[size+1];
int actualBytesRead = fread(memoryBuffer,1,size,file);
if (actualBytesRead!=size)
char* memoryBuffer = new char[size + 1];
int actualBytesRead = fread(memoryBuffer, 1, size, file);
if (actualBytesRead != size)
{
b3Warning("Error reading from file %s",relativeFileName);
} else
b3Warning("Error reading from file %s", relativeFileName);
}
else
{
int numTriangles = *(int*)&memoryBuffer[80];
if (numTriangles)
{
{
//perform a sanity check instead of crashing on invalid triangles/STL files
int expectedBinaryFileSize = numTriangles* 50 + 84;
int expectedBinaryFileSize = numTriangles * 50 + 84;
if (expectedBinaryFileSize != size)
{
delete[] memoryBuffer;
return 0;
}
}
shape = new GLInstanceGraphicsShape;
// b3AlignedObjectArray<GLInstanceVertex>* m_vertices;
// int m_numvertices;
// b3AlignedObjectArray<int>* m_indices;
// int m_numIndices;
// float m_scaling[4];
// b3AlignedObjectArray<GLInstanceVertex>* m_vertices;
// int m_numvertices;
// b3AlignedObjectArray<int>* m_indices;
// int m_numIndices;
// float m_scaling[4];
shape->m_scaling[0] = 1;
shape->m_scaling[1] = 1;
shape->m_scaling[2] = 1;
@@ -64,16 +65,16 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
int index = 0;
shape->m_indices = new b3AlignedObjectArray<int>();
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i=0;i<numTriangles;i++)
for (int i = 0; i < numTriangles; i++)
{
char* curPtr = &memoryBuffer[84+i*50];
char* curPtr = &memoryBuffer[84 + i * 50];
MySTLTriangle tmp;
memcpy(&tmp,curPtr,sizeof(MySTLTriangle));
GLInstanceVertex v0,v1,v2;
memcpy(&tmp, curPtr, sizeof(MySTLTriangle));
GLInstanceVertex v0, v1, v2;
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
for (int v = 0; v < 3; v++)
{
v0.xyzw[v] = tmp.vertex0[v];
v1.xyzw[v] = tmp.vertex1[v];
@@ -81,19 +82,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
shape->m_vertices->push_back(v0);
shape->m_vertices->push_back(v1);
shape->m_vertices->push_back(v2);
shape->m_indices->push_back(index++);
shape->m_indices->push_back(index++);
shape->m_indices->push_back(index++);
}
}
}
delete[] memoryBuffer;
}
}
@@ -107,4 +107,4 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
return shape;
}
#endif //LOAD_MESH_FROM_STL_H
#endif //LOAD_MESH_FROM_STL_H

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

File diff suppressed because it is too large Load Diff

61
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -1,5 +1,5 @@
#ifndef BULLET_URDF_IMPORTER_H
#define BULLET_URDF_IMPORTER_H
#define BULLET_URDF_IMPORTER_H
#include "URDFImporterInterface.h"
@@ -13,21 +13,16 @@ struct BulletURDFTexture
bool m_isCached;
};
///BulletURDFImporter can deal with URDF and (soon) SDF files
class BulletURDFImporter : public URDFImporterInterface
{
struct BulletURDFInternalData* m_data;
public:
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling=1, int flags=0);
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling = 1, int flags = 0);
virtual ~BulletURDFImporter();
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
@@ -38,15 +33,15 @@ public:
virtual int getBodyUniqueId() const;
const char* getPathPrefix();
void printTree(); //for debugging
void printTree(); //for debugging
virtual int getRootLinkIndex() const;
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual std::string getBodyName() const;
virtual std::string getLinkName(int linkIndex) const;
virtual std::string getLinkName(int linkIndex) const;
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
@@ -54,50 +49,46 @@ public:
virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const;
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const;
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
virtual std::string getJointName(int linkIndex) const;
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
virtual std::string getJointName(int linkIndex) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const;
virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
virtual int getNumAllocatedCollisionShapes() const;
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
virtual int getNumAllocatedCollisionShapes() const;
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
virtual int getNumAllocatedMeshInterfaces() const;
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
virtual int getNumAllocatedTextures() const;
virtual int getAllocatedTexture(int index) const;
virtual void setEnableTinyRenderer(bool enable);
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut) const;
};
#endif //BULLET_URDF_IMPORTER_H
#endif //BULLET_URDF_IMPORTER_H

9
examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h
View File

@@ -5,10 +5,9 @@ struct ConvertRigidBodies2MultiBody
{
btAlignedObjectArray<btRigidBody*> m_bodies;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
virtual void addRigidBody(btRigidBody* body);
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
virtual void addRigidBody(btRigidBody* body);
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false);
virtual btMultiBody* convertToMultiBody();
};
#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H
#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H

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

@@ -11,93 +11,82 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
#include "BulletUrdfImporter.h"
#include "URDF2Bullet.h"
//#include "urdf_samples.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "MyMultiBodyCreator.h"
class ImportUrdfSetup : public CommonMultiBodyBase
{
char m_fileName[1024];
char m_fileName[1024];
struct ImportUrdfInternalData* m_data;
struct ImportUrdfInternalData* m_data;
bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory;
btAlignedObjectArray<std::string*> m_nameMemory;
btScalar m_grav;
int m_upAxis;
public:
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportUrdfSetup();
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportUrdfSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void setFileName(const char* urdfFileName);
void setFileName(const char* urdfFileName);
virtual void resetCamera()
{
float dist = 3.5;
float pitch = -28;
float yaw = -136;
float targetPos[3]={0.47,0,-0.64};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
float targetPos[3] = {0.47, 0, -0.64};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
btAlignedObjectArray<std::string> gFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportUrdfInternalData
{
ImportUrdfInternalData()
:m_numMotors(0),
m_mb(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
ImportUrdfInternalData()
: m_numMotors(0),
m_mb(0)
{
for (int i = 0; i < MAX_NUM_MOTORS; i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
btMultiBody* m_mb;
btRigidBody* m_rb;
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
int m_numMotors;
btMultiBody* m_mb;
btRigidBody* m_rb;
};
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper),
m_grav(-10),
m_upAxis(2)
: CommonMultiBodyBase(helper),
m_grav(-10),
m_upAxis(2)
{
m_data = new ImportUrdfInternalData;
if (option==1)
if (option == 1)
{
m_useMultiBody = true;
} else
}
else
{
m_useMultiBody = false;
}
@@ -106,15 +95,14 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (fileName)
{
setFileName(fileName);
} else
}
else
{
gFileNameArray.clear();
//load additional urdf file names from file
FILE* f = fopen("urdf_files.txt","r");
FILE* f = fopen("urdf_files.txt", "r");
if (f)
{
int result;
@@ -122,68 +110,56 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("urdf_files.txt entry %s",fileName);
if (result==1)
result = fscanf(f, "%s", fileName);
b3Printf("urdf_files.txt entry %s", fileName);
if (result == 1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
} while (result == 1);
fclose(f);
}
if (gFileNameArray.size()==0)
if (gFileNameArray.size() == 0)
{
gFileNameArray.push_back("r2d2.urdf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
if (count >= numFileNames)
{
count=0;
count = 0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
sprintf(m_fileName, "%s", gFileNameArray[count++].c_str());
}
}
ImportUrdfSetup::~ImportUrdfSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
for (int i = 0; i < m_nameMemory.size(); i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
delete m_data;
}
void ImportUrdfSetup::setFileName(const char* urdfFileName)
{
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
memcpy(m_fileName, urdfFileName, strlen(urdfFileName) + 1);
}
void ImportUrdfSetup::initPhysics()
{
m_guiHelper->setUpAxis(m_upAxis);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
if (m_guiHelper->getParameterInterface())
{
@@ -192,19 +168,17 @@ void ImportUrdfSetup::initPhysics()
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
int flags=0;
double globalScaling=1;
BulletURDFImporter u2b(m_guiHelper, 0,globalScaling,flags);
int flags = 0;
double globalScaling = 1;
BulletURDFImporter u2b(m_guiHelper, 0, globalScaling, flags);
bool loadOk = u2b.loadURDF(m_fileName);
#ifdef TEST_MULTIBODY_SERIALIZATION
#ifdef TEST_MULTIBODY_SERIALIZATION
//test to serialize a multibody to disk or shared memory, with base, link and joint names
btSerializer* s = new btDefaultSerializer;
#endif //TEST_MULTIBODY_SERIALIZATION
#endif //TEST_MULTIBODY_SERIALIZATION
if (loadOk)
{
@@ -215,18 +189,13 @@ void ImportUrdfSetup::initPhysics()
btTransform identityTrans;
identityTrans.setIdentity();
{
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
//int rootLinkIndex = u2b.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, m_useMultiBody, u2b.getPathPrefix());
m_data->m_rb = creation.getRigidBody();
m_data->m_mb = creation.getBulletMultiBody();
btMultiBody* mb = m_data->m_mb;
@@ -235,146 +204,137 @@ void ImportUrdfSetup::initPhysics()
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
if (m_useMultiBody && mb )
if (m_useMultiBody && mb)
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
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
s->registerNameForPointer(name->c_str(), name->c_str());
#endif //TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(), jointName->c_str());
s->registerNameForPointer(linkName->c_str(), linkName->c_str());
#endif //TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
if (m_data->m_numMotors < MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
sprintf(motorName, "%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
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);
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_data->m_jointMotors[m_data->m_numMotors] = motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
} else
}
else
{
if (1)
{
//create motors for each generic joint
int num6Dof = creation.getNum6DofConstraints();
for (int i=0;i<num6Dof;i++)
for (int i = 0; i < num6Dof; i++)
{
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr())
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType ==URDFContinuousJoint))
if ((jointInfo->m_urdfJointType == URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType == URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType == URDFContinuousJoint))
{
int urdfLinkIndex = jointInfo->m_urdfIndex;
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
sprintf(motorName, "%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
SliderParams slider(motorName, motorVel);
slider.m_minVal = -4;
slider.m_maxVal = 4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
m_data->m_generic6DofJointMotors[m_data->m_numMotors] = c;
bool motorOn = true;
c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
c->enableMotor(jointInfo->m_jointAxisIndex, motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex, 10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex, 0);
m_data->m_numMotors++;
}
}
}
}
}
}
//the btMultiBody support is work-in-progress :-)
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}
for (int i = 0; i < m_dynamicsWorld->getNumMultiBodyConstraints(); i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}
bool createGround=true;
bool createGround = true;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[m_upAxis]=1.f;
btVector3 groundHalfExtents(20, 20, 20);
groundHalfExtents[m_upAxis] = 1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[m_upAxis]=-2.5;
btTransform start;
start.setIdentity();
btVector3 groundOrigin(0, 0, 0);
groundOrigin[m_upAxis] = -2.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btRigidBody* body = createRigidBody(0, start, box);
//m_dynamicsWorld->removeRigidBody(body);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5,0.5,0.5);
m_guiHelper->createRigidBodyGraphicsObject(body,color);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5, 0.5, 0.5);
m_guiHelper->createRigidBodyGraphicsObject(body, color);
}
}
#ifdef TEST_MULTIBODY_SERIALIZATION
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
if (p.findResourcePath("r2d2_multibody.bullet", resourcePath, 1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
FILE* f = fopen(resourcePath, "wb");
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
fclose(f);
}
#endif//TEST_MULTIBODY_SERIALIZATION
#endif //TEST_MULTIBODY_SERIALIZATION
}
void ImportUrdfSetup::stepSimulation(float deltaTime)
@@ -385,8 +345,8 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
gravity[m_upAxis] = m_grav;
m_dynamicsWorld->setGravity(gravity);
for (int i=0;i<m_data->m_numMotors;i++)
{
for (int i = 0; i < m_data->m_numMotors; i++)
{
if (m_data->m_jointMotors[i])
{
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
@@ -394,18 +354,17 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
if (m_data->m_generic6DofJointMotors[i])
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetVelocities[i]);
//jointInfo->
}
}
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
}
}
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options)
{
return new ImportUrdfSetup(options.m_guiHelper, options.m_option,options.m_fileName);
return new ImportUrdfSetup(options.m_guiHelper, options.m_option, options.m_fileName);
}

6
examples/Importers/ImportURDFDemo/ImportURDFSetup.h
View File

@@ -1,8 +1,6 @@
#ifndef IMPORT_URDF_SETUP_H
#define IMPORT_URDF_SETUP_H
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_URDF_SETUP_H
#endif //IMPORT_URDF_SETUP_H

45
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -6,40 +6,37 @@
class MultiBodyCreationInterface
{
public:
virtual ~MultiBodyCreationInterface() {}
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
{
createRigidBodyGraphicsInstance(linkIndex,body,colorRgba,graphicsIndex);
createRigidBodyGraphicsInstance(linkIndex, body, colorRgba, graphicsIndex);
}
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
{
createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
}
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) =0;
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0;
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) = 0;
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder = 0) = 0;
virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit) = 0;
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit) = 0;
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
};
#endif //MULTIBODY_CREATION_INTERFACE_H
#endif //MULTIBODY_CREATION_INTERFACE_H

203
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -12,56 +12,51 @@
#include "URDFJointTypes.h"
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_bulletMultiBody(0),
m_rigidBody(0),
m_guiHelper(guiHelper)
: m_bulletMultiBody(0),
m_rigidBody(0),
m_guiHelper(guiHelper)
{
}
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
{
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
m_mb2urdfLink.resize(totalNumJoints+1,-2);
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
m_mb2urdfLink.resize(totalNumJoints + 1, -2);
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
m_bulletMultiBody = new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
//if (canSleep)
// m_bulletMultiBody->goToSleep();
return m_bulletMultiBody;
return m_bulletMultiBody;
}
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
m_rigidBody = new btRigidBody(rbci);
return m_rigidBody;
}
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
{
btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
return mbCol;
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
m_rigidBody = new btRigidBody(rbci);
return m_rigidBody;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
{
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
return c;
btMultiBodyLinkCollider* mbCol = new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
return mbCol;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
{
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA, rbB, offsetInA, offsetInB, (RotateOrder)rotateOrder);
return c;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
{
int rotateOrder=0;
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA , rbB, offsetInA, offsetInB, rotateOrder);
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
int rotateOrder = 0;
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, rotateOrder);
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
int principleAxis = jointAxisInJointSpace.closestAxis();
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
@@ -74,89 +69,86 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(i
userInfo->m_urdfIndex = urdfLinkIndex;
dof6->setUserConstraintPtr(userInfo);
switch (principleAxis)
{
case 0:
{
dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
dof6->setLinearLowerLimit(btVector3(jointLowerLimit, 0, 0));
dof6->setLinearUpperLimit(btVector3(jointUpperLimit, 0, 0));
break;
}
case 1:
{
dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
dof6->setLinearLowerLimit(btVector3(0, jointLowerLimit, 0));
dof6->setLinearUpperLimit(btVector3(0, jointUpperLimit, 0));
break;
}
case 2:
default:
{
dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
dof6->setLinearLowerLimit(btVector3(0, 0, jointLowerLimit));
dof6->setLinearUpperLimit(btVector3(0, 0, jointUpperLimit));
}
};
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0, 0, 0));
dof6->setAngularUpperLimit(btVector3(0, 0, 0));
m_6DofConstraints.push_back(dof6);
return dof6;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
{
btGeneric6DofSpring2Constraint* dof6 = 0;
//only handle principle axis at the moment,
//@todo(erwincoumans) orient the constraint for non-principal axis
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_ZYX);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
//only handle principle axis at the moment,
//@todo(erwincoumans) orient the constraint for non-principal axis
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_ZYX);
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
dof6->setAngularLowerLimit(btVector3(jointLowerLimit,0,0));
dof6->setAngularUpperLimit(btVector3(jointUpperLimit,0,0));
dof6->setAngularLowerLimit(btVector3(jointLowerLimit, 0, 0));
dof6->setAngularUpperLimit(btVector3(jointUpperLimit, 0, 0));
break;
}
case 1:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XZY);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
break;
}
case 1:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XZY);
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
dof6->setAngularLowerLimit(btVector3(0,jointLowerLimit,0));
dof6->setAngularUpperLimit(btVector3(0,jointUpperLimit,0));
break;
}
case 2:
default:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XYZ);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0, jointLowerLimit, 0));
dof6->setAngularUpperLimit(btVector3(0, jointUpperLimit, 0));
break;
}
case 2:
default:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XYZ);
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
dof6->setAngularLowerLimit(btVector3(0,0,jointLowerLimit));
dof6->setAngularUpperLimit(btVector3(0,0,jointUpperLimit));
}
};
dof6->setAngularLowerLimit(btVector3(0, 0, jointLowerLimit));
dof6->setAngularUpperLimit(btVector3(0, 0, jointUpperLimit));
}
};
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
userInfo->m_jointAxisIndex = 3+principleAxis;
userInfo->m_jointAxisIndex = 3 + principleAxis;
if (jointLowerLimit > jointUpperLimit)
{
userInfo->m_urdfJointType = URDFContinuousJoint;
} else
}
else
{
userInfo->m_urdfJointType = URDFRevoluteJoint;
userInfo->m_lowerJointLimit = jointLowerLimit;
@@ -168,73 +160,64 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in
return dof6;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB)
{
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB);
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
userInfo->m_urdfIndex = urdfLinkIndex;
userInfo->m_urdfJointType = URDFFixedJoint;
dof6->setUserConstraintPtr(userInfo);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0, 0, 0));
dof6->setAngularUpperLimit(btVector3(0, 0, 0));
m_6DofConstraints.push_back(dof6);
return dof6;
}
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
{
if (m_mb2urdfLink.size()<(mbLinkIndex+1))
if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
{
m_mb2urdfLink.resize((mbLinkIndex+1),-2);
m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
}
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
}
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
{
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
}
void MyMultiBodyCreator::createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
{
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
{
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
int graphicsInstanceId = body->getUserIndex();
btVector3DoubleData speculard;
specularColor.serializeDouble(speculard);
m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
}
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
{
m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
m_guiHelper->createCollisionObjectGraphicsObject(colObj, colorRgba);
}
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
{
createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
int graphicsInstanceId = col->getUserIndex();
btVector3DoubleData speculard;
specularColor.serializeDouble(speculard);
m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
}
btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
{
return m_bulletMultiBody;
return m_bulletMultiBody;
}

66
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -11,74 +11,70 @@ class btMultiBody;
struct GenericConstraintUserInfo
{
int m_urdfIndex;
int m_urdfIndex;
int m_urdfJointType;
btVector3 m_jointAxisInJointSpace;
int m_jointAxisIndex;
int m_jointAxisIndex;
btScalar m_lowerJointLimit;
btScalar m_upperJointLimit;
};
class MyMultiBodyCreator : public MultiBodyCreationInterface
{
protected:
btMultiBody* m_bulletMultiBody;
btRigidBody* m_rigidBody;
struct GUIHelperInterface* m_guiHelper;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
public:
btAlignedObjectArray<int> m_mb2urdfLink;
btAlignedObjectArray<int> m_mb2urdfLink;
MyMultiBodyCreator(GUIHelperInterface* guiHelper);
virtual ~MyMultiBodyCreator() {}
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ;
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex) ;
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex);
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex);
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder = 0);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0);
virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit);
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit);
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody();
btRigidBody* getRigidBody()
{
return m_rigidBody;
}
int getNum6DofConstraints() const
return m_rigidBody;
}
int getNum6DofConstraints() const
{
return m_6DofConstraints.size();
}
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
{
return m_6DofConstraints[index];
}
};
#endif //MY_MULTIBODY_CREATOR
#endif //MY_MULTIBODY_CREATOR

41
examples/Importers/ImportURDFDemo/SDFAudioTypes.h
View File

@@ -3,43 +3,42 @@
#include <string>
///See audio_source element in http://sdformat.org/spec?ver=1.6&elem=link
struct SDFAudioSource
{
enum
{
SDFAudioSourceValid=1,
SDFAudioSourceLooping=2,
SDFAudioSourceValid = 1,
SDFAudioSourceLooping = 2,
};
int m_flags; //repeat mode (0 = no repeat, 1 = loop forever)
int m_flags; //repeat mode (0 = no repeat, 1 = loop forever)
std::string m_uri; //media filename of the sound, .wav file
double m_pitch; //1 = regular rate, -1 play in reverse
double m_gain; //normalized volume in range [0..1] where 0 is silent, 1 is most loud
std::string m_uri; //media filename of the sound, .wav file
double m_pitch; //1 = regular rate, -1 play in reverse
double m_gain; //normalized volume in range [0..1] where 0 is silent, 1 is most loud
double m_attackRate;
double m_decayRate;
double m_sustainLevel;
double m_releaseRate;
double m_collisionForceThreshold; //force that will trigger the audio, in Newton. If < 0, audio source is invalid
double m_collisionForceThreshold; //force that will trigger the audio, in Newton. If < 0, audio source is invalid
int m_userIndex;
SDFAudioSource()
: m_flags(0),
m_pitch(1),
m_gain(1),
m_attackRate(0.0001),
m_decayRate(0.00001),
m_sustainLevel(0.5),
m_releaseRate(0.0005),
m_collisionForceThreshold(0.5),
m_userIndex(-1)
{
}
: m_flags(0),
m_pitch(1),
m_gain(1),
m_attackRate(0.0001),
m_decayRate(0.00001),
m_sustainLevel(0.5),
m_releaseRate(0.0005),
m_collisionForceThreshold(0.5),
m_userIndex(-1)
{
}
};
#endif //SDF_AUDIO_TYPES_H
#endif //SDF_AUDIO_TYPES_H

756
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -4,7 +4,6 @@
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
@@ -20,131 +19,122 @@
static bool enableConstraints = true;
static btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
static btVector4 selectColor2()
{
static int curColor = 0;
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
return color;
static int curColor = 0;
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
return color;
}
struct URDF2BulletCachedData
{
URDF2BulletCachedData()
:
m_currentMultiBodyLinkIndex(-1),
m_bulletMultiBody(0),
m_totalNumJoints1(0)
{
URDF2BulletCachedData()
: m_currentMultiBodyLinkIndex(-1),
m_bulletMultiBody(0),
m_totalNumJoints1(0)
{
}
//these arrays will be initialized in the 'InitURDF2BulletCache'
}
//these arrays will be initialized in the 'InitURDF2BulletCache'
btAlignedObjectArray<int> m_urdfLinkParentIndices;
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
btAlignedObjectArray<int> m_urdfLinkParentIndices;
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
int m_currentMultiBodyLinkIndex;
int m_currentMultiBodyLinkIndex;
class btMultiBody* m_bulletMultiBody;
class btMultiBody* m_bulletMultiBody;
//this will be initialized in the constructor
int m_totalNumJoints1;
int getParentUrdfIndex(int linkIndex) const
{
return m_urdfLinkParentIndices[linkIndex];
}
int getMbIndexFromUrdfIndex(int urdfIndex) const
{
if (urdfIndex == -2)
return -2;
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
}
//this will be initialized in the constructor
int m_totalNumJoints1;
int getParentUrdfIndex(int linkIndex) const
{
return m_urdfLinkParentIndices[linkIndex];
}
int getMbIndexFromUrdfIndex(int urdfIndex) const
{
if (urdfIndex==-2)
return -2;
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
}
void registerMultiBody(int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
{
return m_urdfLink2rigidBodies[urdfLinkIndex];
}
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
{
return m_urdfLink2rigidBodies[urdfLinkIndex];
}
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
void registerRigidBody(int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex] == 0);
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
};
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
{
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
//for (int i=0;i<childIndices.size();i++)
//{
// b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
//}
cache.m_totalNumJoints1 += childIndices.size();
for (int i=0;i<childIndices.size();i++)
{
int childIndex =childIndices[i];
ComputeTotalNumberOfJoints(u2b,cache,childIndex);
}
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex, childIndices);
//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
//for (int i=0;i<childIndices.size();i++)
//{
// b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
//}
cache.m_totalNumJoints1 += childIndices.size();
for (int i = 0; i < childIndices.size(); i++)
{
int childIndex = childIndices[i];
ComputeTotalNumberOfJoints(u2b, cache, childIndex);
}
}
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
{
cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
for (int i=0;i<childIndices.size();i++)
{
ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
}
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
for (int i = 0; i < childIndices.size(); i++)
{
ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
}
}
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
{
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
int rootLinkIndex = u2b.getRootLinkIndex();
if (rootLinkIndex>=0)
{
ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;
int rootLinkIndex = u2b.getRootLinkIndex();
if (rootLinkIndex >= 0)
{
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
}
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
}
}
void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col)
@@ -176,106 +166,96 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
}
}
btScalar tmpUrdfScaling=2;
btScalar tmpUrdfScaling = 2;
void ConvertURDF2BulletInternal(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
URDF2BulletCachedData& cache, int urdfLinkIndex,
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix,
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn=0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut=0)
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
URDF2BulletCachedData& cache, int urdfLinkIndex,
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix,
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut = 0)
{
B3_PROFILE("ConvertURDF2BulletInternal2");
//b3Printf("start converting/extracting data from URDF interface\n");
//b3Printf("start converting/extracting data from URDF interface\n");
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
//b3Printf("mb link index = %d\n",mbLinkIndex);
//b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
btScalar parentMass(1);
btVector3 parentLocalInertiaDiagonal(1,1,1);
btVector3 parentLocalInertiaDiagonal(1, 1, 1);
if (urdfParentIndex==-2)
{
//b3Printf("root link has no parent\n");
} else
{
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
//b3Printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia2(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame, flags);
if (urdfParentIndex == -2)
{
//b3Printf("root link has no parent\n");
}
else
{
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
//b3Printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
}
}
btScalar mass = 0;
btTransform localInertialFrame;
localInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0, 0, 0);
u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
btScalar mass = 0;
btTransform localInertialFrame;
localInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0,0,0);
u2b.getMassAndInertia2(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame, flags);
btTransform parent2joint;
parent2joint.setIdentity();
btTransform parent2joint;
parent2joint.setIdentity();
int jointType;
btVector3 jointAxisInJointSpace;
btScalar jointLowerLimit;
btScalar jointUpperLimit;
btScalar jointDamping;
btScalar jointFriction;
int jointType;
btVector3 jointAxisInJointSpace;
btScalar jointLowerLimit;
btScalar jointUpperLimit;
btScalar jointDamping;
btScalar jointFriction;
btScalar jointMaxForce;
btScalar jointMaxVelocity;
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
std::string linkName = u2b.getLinkName(urdfLinkIndex);
if (flags & CUF_USE_SDF)
{
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
}
else
{
if (flags & CUF_USE_SDF)
{
parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
}
else
{
if (flags & CUF_USE_MJCF)
{
linkTransformInWorldSpace =parentTransformInWorldSpace*linkTransformInWorldSpace;
} else
{
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
}
}
else
{
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
}
}
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
btCollisionShape* compoundShape = tmpShape;
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity())
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0) == btTransform::getIdentity())
{
compoundShape = tmpShape->getChildShape(0);
}
int graphicsIndex;
{
B3_PROFILE("convertLinkVisualShapes");
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex+1))
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
{
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex+1];
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
u2b.setLinkColor2(urdfLinkIndex, matColor);
}
@@ -291,85 +271,77 @@ void ConvertURDF2BulletInternal(
}
}
}
if (compoundShape)
{
if (compoundShape)
{
UrdfMaterialColor matColor;
btVector4 color2 = selectColor2();
btVector3 specular(0.5,0.5,0.5);
if (u2b.getLinkColor2(urdfLinkIndex,matColor))
btVector4 color2 = selectColor2();
btVector3 specular(0.5, 0.5, 0.5);
if (u2b.getLinkColor2(urdfLinkIndex, matColor))
{
color2 = matColor.m_rgbaColor;
specular = matColor.m_specularColor;
}
/*
/*
if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
if (mass)
{
if (!(flags & CUF_USE_URDF_INERTIA))
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
btAssert(localInertiaDiagonal[0] < 1e10);
btAssert(localInertiaDiagonal[1] < 1e10);
btAssert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
//temporary inertia scaling until we load inertia from URDF
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
{
localInertiaDiagonal*=contactInfo.m_inertiaScaling;
}
}
if (mass)
{
if (!(flags & CUF_USE_URDF_INERTIA))
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
btAssert(localInertiaDiagonal[0] < 1e10);
btAssert(localInertiaDiagonal[1] < 1e10);
btAssert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
//temporary inertia scaling until we load inertia from URDF
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
{
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
}
}
btRigidBody* linkRigidBody = 0;
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
btRigidBody* linkRigidBody = 0;
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
if (!createMultiBody)
{
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
if (!createMultiBody)
{
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
if (!canSleep)
{
body->forceActivationState(DISABLE_DEACTIVATION);
}
linkRigidBody = body;
linkRigidBody = body;
world1->addRigidBody(body);
world1->addRigidBody(body);
compoundShape->setUserIndex(graphicsIndex);
compoundShape->setUserIndex(graphicsIndex);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
processContactParameters(contactInfo, body);
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2,specular, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
} else
{
if (cache.m_bulletMultiBody==0)
{
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1;
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
}
else
{
if (cache.m_bulletMultiBody == 0)
{
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1;
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
if (flags & CUF_GLOBAL_VELOCITIES_MB)
{
cache.m_bulletMultiBody->useGlobalVelocities(true);
@@ -378,21 +350,20 @@ void ConvertURDF2BulletInternal(
{
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
}
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
}
}
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
}
}
//create a joint if necessary
if (hasParentJoint) {
//create a joint if necessary
if (hasParentJoint)
{
btTransform offsetInA, offsetInB;
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
offsetInB = localInertialFrame.inverse();
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
btTransform offsetInA,offsetInB;
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
offsetInB = localInertialFrame.inverse();
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
bool disableParentCollision = true;
bool disableParentCollision = true;
if (createMultiBody && cache.m_bulletMultiBody)
{
@@ -404,103 +375,108 @@ void ConvertURDF2BulletInternal(
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
}
switch (jointType)
{
switch (jointType)
{
case URDFFloatingJoint:
case URDFPlanarJoint:
case URDFFixedJoint:
{
if ((jointType==URDFFloatingJoint)||(jointType==URDFPlanarJoint))
case URDFFixedJoint:
{
if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
{
printf("Warning: joint unsupported, creating a fixed joint instead.");
}
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin());
}
else
{
//b3Printf("Fixed joint\n");
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
} else
{
//b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = 0;
//backward compatibility
if (flags & CUF_RESERVED )
if (flags & CUF_RESERVED)
{
dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
} else
{
dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
}
if (enableConstraints)
world1->addConstraint(dof6,true);
}
break;
}
case URDFContinuousJoint:
case URDFRevoluteJoint:
{
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (createMultiBody)
{
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
} else
{
else
{
dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
}
if (enableConstraints)
world1->addConstraint(dof6, true);
}
break;
}
case URDFContinuousJoint:
case URDFRevoluteJoint:
{
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
btGeneric6DofSpring2Constraint* dof6 = 0;
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
}
else
{
btGeneric6DofSpring2Constraint* dof6 = 0;
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
{
//backwards compatibility
if (flags & CUF_RESERVED )
if (flags & CUF_RESERVED)
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
} else
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
}
} else
{
//disable joint limits
if (flags & CUF_RESERVED )
else
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,1,-1);
} else
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,1,-1);
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
}
}
else
{
//disable joint limits
if (flags & CUF_RESERVED)
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, 1, -1);
}
else
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, 1, -1);
}
}
if (enableConstraints)
world1->addConstraint(dof6,true);
//b3Printf("Revolute/Continuous joint\n");
}
break;
}
case URDFPrismaticJoint:
{
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (createMultiBody)
{
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
if (enableConstraints)
world1->addConstraint(dof6, true);
//b3Printf("Revolute/Continuous joint\n");
}
break;
}
case URDFPrismaticJoint:
{
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
if (jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
@@ -509,38 +485,36 @@ void ConvertURDF2BulletInternal(
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
}
else
{
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
} else
{
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints)
world1->addConstraint(dof6,true);
if (enableConstraints)
world1->addConstraint(dof6, true);
//b3Printf("Prismatic\n");
}
break;
}
default:
{
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
//b3Printf("Prismatic\n");
}
break;
}
default:
{
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
btAssert(0);
}
}
}
}
}
}
if (createMultiBody)
{
//if (compoundShape->getNumChildShapes()>0)
{
btMultiBodyLinkCollider* col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
if (createMultiBody)
{
//if (compoundShape->getNumChildShapes()>0)
{
btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
compoundShape->setUserIndex(graphicsIndex);
compoundShape->setUserIndex(graphicsIndex);
col->setCollisionShape(compoundShape);
col->setCollisionShape(compoundShape);
if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
@@ -551,21 +525,21 @@ void ConvertURDF2BulletInternal(
}
}
btTransform tr;
tr.setIdentity();
tr = linkTransformInWorldSpace;
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
btTransform tr;
tr.setIdentity();
tr = linkTransformInWorldSpace;
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
col->setWorldTransform(tr);
col->setWorldTransform(tr);
//base and fixed? -> static, otherwise flag as dynamic
bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody->hasFixedBase()) ? false : true;
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int colGroup=0, colMask=0;
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
int colGroup = 0, colMask = 0;
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, colGroup, colMask);
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
@@ -574,12 +548,12 @@ void ConvertURDF2BulletInternal(
{
collisionFilterMask = colMask;
}
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
btVector4 color2 = selectColor2();//(0.0,0.0,0.5);
btVector3 specularColor(1,1,1);
btVector4 color2 = selectColor2(); //(0.0,0.0,0.5);
btVector3 specularColor(1, 1, 1);
UrdfMaterialColor matCol;
if (u2b.getLinkColor2(urdfLinkIndex,matCol))
if (u2b.getLinkColor2(urdfLinkIndex, matCol))
{
color2 = matCol.m_rgbaColor;
specularColor = matCol.m_specularColor;
@@ -593,75 +567,72 @@ void ConvertURDF2BulletInternal(
u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
processContactParameters(contactInfo, col);
if (mbLinkIndex>=0) //???? double-check +/- 1
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
if (flags&CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
if (mbLinkIndex >= 0) //???? double-check +/- 1
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col;
if (flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags &= ~BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
}
if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
if (flags & CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
}
} else
{
// if (canSleep)
}
else
{
// if (canSleep)
{
if (cache.m_bulletMultiBody->getBaseMass()==0)
//&& cache.m_bulletMultiBody->getNumDofs()==0)
if (cache.m_bulletMultiBody->getBaseMass() == 0)
//&& cache.m_bulletMultiBody->getNumDofs()==0)
{
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
}
}
cache.m_bulletMultiBody->setBaseCollider(col);
}
}
} else
{
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
cache.m_bulletMultiBody->setBaseCollider(col);
}
}
}
else
{
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
//u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
u2b.convertLinkVisualShapes2(-1,urdfLinkIndex,pathPrefix,localInertialFrame,linkRigidBody,u2b.getBodyUniqueId());
}
}
u2b.convertLinkVisualShapes2(-1, urdfLinkIndex, pathPrefix, localInertialFrame, linkRigidBody, u2b.getBodyUniqueId());
}
}
btAlignedObjectArray<int> urdfChildIndices;
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
btAlignedObjectArray<int> urdfChildIndices;
u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
int numChildren = urdfChildIndices.size();
int numChildren = urdfChildIndices.size();
for (int i=0;i<numChildren;i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
}
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
}
}
void ConvertURDF2Bullet(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
InitURDF2BulletCache(u2b, cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
B3_PROFILE("ConvertURDF2Bullet");
UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
{
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
@@ -672,25 +643,24 @@ void ConvertURDF2Bullet(
B3_PROFILE("Post process");
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0);
mb->setHasSelfCollision((flags & CUF_USE_SELF_COLLISION) != 0);
mb->finalizeMultiDof();
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
if (flags & CUF_USE_MJCF)
{
} else
}
else
{
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
mb->setBaseWorldTransform(rootTransformInWorldSpace * localInertialFrameRoot);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
mb->forwardKinematics(scratch_q, scratch_m);
mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
world1->addMultiBody(mb);
}
}

57
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -3,35 +3,34 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include <string>
#include "URDFJointTypes.h"//for UrdfMaterialColor cache
#include "URDFJointTypes.h" //for UrdfMaterialColor cache
class btVector3;
class btTransform;
class btMultiBodyDynamicsWorld;
class btTransform;
class URDFImporterInterface;
class MultiBodyCreationInterface;
//manually sync with eURDF_Flags in SharedMemoryPublic.h!
enum ConvertURDFFlags {
CUF_USE_SDF = 1,
// Use inertia values in URDF instead of recomputing them from collision shape.
CUF_USE_URDF_INERTIA = 2,
CUF_USE_MJCF = 4,
CUF_USE_SELF_COLLISION=8,
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16,
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
CUF_RESERVED=64,
CUF_USE_IMPLICIT_CYLINDER=128,
CUF_GLOBAL_VELOCITIES_MB=256,
CUF_MJCF_COLORS_FROM_FILE=512,
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
CUF_ENABLE_SLEEPING=2048,
CUF_INITIALIZE_SAT_FEATURES=4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
enum ConvertURDFFlags
{
CUF_USE_SDF = 1,
// Use inertia values in URDF instead of recomputing them from collision shape.
CUF_USE_URDF_INERTIA = 2,
CUF_USE_MJCF = 4,
CUF_USE_SELF_COLLISION = 8,
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
CUF_RESERVED = 64,
CUF_USE_IMPLICIT_CYLINDER = 128,
CUF_GLOBAL_VELOCITIES_MB = 256,
CUF_MJCF_COLORS_FROM_FILE = 512,
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
CUF_ENABLE_SLEEPING = 2048,
CUF_INITIALIZE_SAT_FEATURES = 4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
};
struct UrdfVisualShapeCache
@@ -40,17 +39,13 @@ struct UrdfVisualShapeCache
btAlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
};
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
bool createMultiBody,
const char* pathPrefix,
int flags = 0,
UrdfVisualShapeCache* cachedLinkGraphicsShapes= 0
);
#endif //_URDF2BULLET_H
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
bool createMultiBody,
const char* pathPrefix,
int flags = 0,
UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
#endif //_URDF2BULLET_H

93
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -9,93 +9,88 @@
class URDFImporterInterface
{
public:
virtual ~URDFImporterInterface() {}
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0;
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) = 0;
virtual const char* getPathPrefix()=0;
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const = 0;
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const =0;
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; }
virtual const char* getPathPrefix() = 0;
///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const = 0;
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const = 0;
//various derived class in internal source code break with new pure virtual methods, so provide some default implementation
virtual std::string getBodyName() const
{
return "";
}
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false;}
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false; }
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false; }
virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const {}
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0;}
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0; }
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const { return false;}
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo) const { return false; }
virtual std::string getJointName(int linkIndex) const = 0;
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const { return false; }
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia (int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0;
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const
virtual std::string getJointName(int linkIndex) const = 0;
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const = 0;
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const
{
getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame);
}
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const = 0;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const = 0;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
{
//backwards compatibility for custom file importers
jointMaxForce = 0;
jointMaxVelocity = 0;
return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
};
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld){}
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const = 0;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) {}
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
virtual void setBodyUniqueId(int bodyId) {}
virtual int getBodyUniqueId() const { return 0;}
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1; }
//default implementation for backward compatibility
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const {}
virtual void setBodyUniqueId(int bodyId) {}
virtual int getBodyUniqueId() const { return 0; }
//default implementation for backward compatibility
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
virtual int getUrdfFromCollisionShape(const class btCollisionShape* collisionShape, struct UrdfCollision& collision) const
{
return 0;
}
virtual int getNumAllocatedCollisionShapes() const { return 0;}
virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/ ) {return 0;}
virtual int getNumModels() const {return 0;}
virtual void activateModel(int /*modelIndex*/) { }
virtual int getNumAllocatedMeshInterfaces() const { return 0;}
virtual int getNumAllocatedCollisionShapes() const { return 0; }
virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; }
virtual int getNumModels() const { return 0; }
virtual void activateModel(int /*modelIndex*/) {}
virtual int getNumAllocatedMeshInterfaces() const { return 0; }
virtual int getNumAllocatedTextures() const { return 0; }
virtual int getAllocatedTexture(int index) const { return 0; }
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) {return 0;}
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) { return 0; }
};
#endif //URDF_IMPORTER_INTERFACE_H
#endif //URDF_IMPORTER_INTERFACE_H

68
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -5,52 +5,52 @@
enum UrdfJointTypes
{
URDFRevoluteJoint=1,
URDFPrismaticJoint,
URDFContinuousJoint,
URDFFloatingJoint,
URDFPlanarJoint,
URDFFixedJoint,
URDFRevoluteJoint = 1,
URDFPrismaticJoint,
URDFContinuousJoint,
URDFFloatingJoint,
URDFPlanarJoint,
URDFFixedJoint,
};
enum URDF_LinkContactFlags
{
URDF_CONTACT_HAS_LATERAL_FRICTION=1,
URDF_CONTACT_HAS_INERTIA_SCALING=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8,
URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
URDF_CONTACT_HAS_RESTITUTION=128,
URDF_CONTACT_HAS_FRICTION_ANCHOR=256,
URDF_CONTACT_HAS_LATERAL_FRICTION = 1,
URDF_CONTACT_HAS_INERTIA_SCALING = 2,
URDF_CONTACT_HAS_CONTACT_CFM = 4,
URDF_CONTACT_HAS_CONTACT_ERP = 8,
URDF_CONTACT_HAS_STIFFNESS_DAMPING = 16,
URDF_CONTACT_HAS_ROLLING_FRICTION = 32,
URDF_CONTACT_HAS_SPINNING_FRICTION = 64,
URDF_CONTACT_HAS_RESTITUTION = 128,
URDF_CONTACT_HAS_FRICTION_ANCHOR = 256,
};
struct URDFLinkContactInfo
{
btScalar m_lateralFriction;
btScalar m_rollingFriction;
btScalar m_spinningFriction;
btScalar m_spinningFriction;
btScalar m_restitution;
btScalar m_inertiaScaling;
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
btScalar m_contactStiffness;
btScalar m_contactDamping;
int m_flags;
URDFLinkContactInfo()
:m_lateralFriction(0.5),
m_rollingFriction(0),
m_spinningFriction(0),
m_restitution(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0),
m_contactStiffness(1e4),
m_contactDamping(1)
: m_lateralFriction(0.5),
m_rollingFriction(0),
m_spinningFriction(0),
m_restitution(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0),
m_contactStiffness(1e4),
m_contactDamping(1)
{
m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
}
@@ -58,9 +58,9 @@ struct URDFLinkContactInfo
enum UrdfCollisionFlags
{
URDF_FORCE_CONCAVE_TRIMESH=1,
URDF_HAS_COLLISION_GROUP=2,
URDF_HAS_COLLISION_MASK=4,
URDF_FORCE_CONCAVE_TRIMESH = 1,
URDF_HAS_COLLISION_GROUP = 2,
URDF_HAS_COLLISION_MASK = 4,
};
struct UrdfMaterialColor
@@ -68,10 +68,10 @@ struct UrdfMaterialColor
btVector4 m_rgbaColor;
btVector3 m_specularColor;
UrdfMaterialColor()
:m_rgbaColor(0.8, 0.8, 0.8, 1),
m_specularColor(0.4,0.4,0.4)
: m_rgbaColor(0.8, 0.8, 0.8, 1),
m_specularColor(0.4, 0.4, 0.4)
{
}
};
#endif //URDF_JOINT_TYPES_H
#endif //URDF_JOINT_TYPES_H

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

File diff suppressed because it is too large Load Diff

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

@@ -12,14 +12,12 @@
struct ErrorLogger
{
virtual ~ErrorLogger(){}
virtual void reportError(const char* error)=0;
virtual void reportWarning(const char* warning)=0;
virtual void printMessage(const char* msg)=0;
virtual ~ErrorLogger() {}
virtual void reportError(const char* error) = 0;
virtual void reportWarning(const char* warning) = 0;
virtual void printMessage(const char* msg) = 0;
};
struct UrdfMaterial
{
std::string m_name;
@@ -37,38 +35,37 @@ struct UrdfInertia
bool m_hasLinkLocalFrame;
double m_mass;
double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz;
double m_ixx, m_ixy, m_ixz, m_iyy, m_iyz, m_izz;
UrdfInertia()
{
m_hasLinkLocalFrame = false;
m_linkLocalFrame.setIdentity();
m_mass = 0.f;
m_ixx=m_ixy=m_ixz=m_iyy=m_iyz=m_izz=0.f;
m_ixx = m_ixy = m_ixz = m_iyy = m_iyz = m_izz = 0.f;
}
};
enum UrdfGeomTypes
{
URDF_GEOM_SPHERE=2,
URDF_GEOM_SPHERE = 2,
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
URDF_GEOM_MESH,
URDF_GEOM_PLANE,
URDF_GEOM_CAPSULE, //non-standard URDF
URDF_GEOM_CDF,//signed-distance-field, non-standard URDF
URDF_GEOM_UNKNOWN,
URDF_GEOM_CAPSULE, //non-standard URDF
URDF_GEOM_CDF, //signed-distance-field, non-standard URDF
URDF_GEOM_UNKNOWN,
};
struct UrdfGeometry
{
UrdfGeomTypes m_type;
double m_sphereRadius;
btVector3 m_boxSize;
double m_capsuleRadius;
double m_capsuleHeight;
int m_hasFromTo;
@@ -76,42 +73,42 @@ struct UrdfGeometry
btVector3 m_capsuleTo;
btVector3 m_planeNormal;
enum {
FILE_STL =1,
FILE_COLLADA =2,
FILE_OBJ =3,
enum
{
FILE_STL = 1,
FILE_COLLADA = 2,
FILE_OBJ = 3,
FILE_CDF = 4,
};
int m_meshFileType;
int m_meshFileType;
std::string m_meshFileName;
btVector3 m_meshScale;
btVector3 m_meshScale;
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;
UrdfGeometry()
:m_type(URDF_GEOM_UNKNOWN),
m_sphereRadius(1),
m_boxSize(1,1,1),
m_capsuleRadius(1),
m_capsuleHeight(1),
m_hasFromTo(0),
m_capsuleFrom(0,1,0),
m_capsuleTo(1,0,0),
m_planeNormal(0,0,1),
m_meshFileType(0),
m_meshScale(1,1,1),
m_hasLocalMaterial(false)
: m_type(URDF_GEOM_UNKNOWN),
m_sphereRadius(1),
m_boxSize(1, 1, 1),
m_capsuleRadius(1),
m_capsuleHeight(1),
m_hasFromTo(0),
m_capsuleFrom(0, 1, 0),
m_capsuleTo(1, 0, 0),
m_planeNormal(0, 0, 1),
m_meshFileType(0),
m_meshScale(1, 1, 1),
m_hasLocalMaterial(false)
{
}
};
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
struct UrdfShape
{
@@ -121,18 +118,18 @@ struct UrdfShape
std::string m_name;
};
struct UrdfVisual: UrdfShape
struct UrdfVisual : UrdfShape
{
std::string m_materialName;
};
struct UrdfCollision: UrdfShape
struct UrdfCollision : UrdfShape
{
int m_flags;
int m_collisionGroup;
int m_collisionMask;
UrdfCollision()
:m_flags(0)
: m_flags(0)
{
}
};
@@ -141,30 +138,29 @@ struct UrdfJoint;
struct UrdfLink
{
std::string m_name;
UrdfInertia m_inertia;
btTransform m_linkTransformInWorld;
std::string m_name;
UrdfInertia m_inertia;
btTransform m_linkTransformInWorld;
btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink;
UrdfJoint* m_parentJoint;
btArray<UrdfJoint*> m_childJoints;
btArray<UrdfLink*> m_childLinks;
int m_linkIndex;
URDFLinkContactInfo m_contactInfo;
SDFAudioSource m_audioSource;
UrdfLink()
:m_parentLink(0),
m_parentJoint(0),
m_linkIndex(-2)
: m_parentLink(0),
m_parentJoint(0),
m_linkIndex(-2)
{
}
};
struct UrdfJoint
{
@@ -174,22 +170,22 @@ struct UrdfJoint
std::string m_parentLinkName;
std::string m_childLinkName;
btVector3 m_localJointAxis;
double m_lowerLimit;
double m_upperLimit;
double m_effortLimit;
double m_velocityLimit;
double m_jointDamping;
double m_jointFriction;
UrdfJoint()
:m_lowerLimit(0),
m_upperLimit(-1),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),
m_jointFriction(0)
: m_lowerLimit(0),
m_upperLimit(-1),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),
m_jointFriction(0)
{
}
};
@@ -198,16 +194,16 @@ struct UrdfModel
{
std::string m_name;
std::string m_sourceFile;
btTransform m_rootTransformInWorld;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
:m_overrideFixedBase(false)
: m_overrideFixedBase(false)
{
m_rootTransformInWorld.setIdentity();
}
@@ -246,99 +242,96 @@ struct UrdfModel
namespace tinyxml2
{
class XMLElement;
class XMLElement;
};
class UrdfParser
{
protected:
UrdfModel m_urdf2Model;
btAlignedObjectArray<UrdfModel*> m_sdfModels;
btAlignedObjectArray<UrdfModel*> m_tmpModels;
bool m_parseSDF;
int m_activeSdfModel;
UrdfModel m_urdf2Model;
btAlignedObjectArray<UrdfModel*> m_sdfModels;
btAlignedObjectArray<UrdfModel*> m_tmpModels;
bool m_parseSDF;
int m_activeSdfModel;
btScalar m_urdfScaling;
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement *config, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement *config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement *config, ErrorLogger* logger);
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
public:
UrdfParser();
virtual ~UrdfParser();
void setParseSDF(bool useSDF)
{
m_parseSDF = useSDF;
}
bool getParseSDF() const
{
return m_parseSDF;
}
void setParseSDF(bool useSDF)
{
m_parseSDF = useSDF;
}
bool getParseSDF() const
{
return m_parseSDF;
}
void setGlobalScaling(btScalar scaling)
{
m_urdfScaling = scaling;
}
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
bool loadSDF(const char* sdfText, ErrorLogger* logger);
int getNumModels() const
{
//user should have loaded an SDF when calling this method
if (m_parseSDF)
{
return m_sdfModels.size();
}
return 1;
}
void activateModel(int modelIndex);
UrdfModel& getModelByIndex(int index)
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
bool loadSDF(const char* sdfText, ErrorLogger* logger);
return *m_sdfModels[index];
}
const UrdfModel& getModelByIndex(int index) const
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
return *m_sdfModels[index];
}
const UrdfModel& getModel() const
int getNumModels() const
{
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
//user should have loaded an SDF when calling this method
if (m_parseSDF)
{
return m_sdfModels.size();
}
return 1;
}
void activateModel(int modelIndex);
UrdfModel& getModelByIndex(int index)
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
return *m_sdfModels[index];
}
const UrdfModel& getModelByIndex(int index) const
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
return *m_sdfModels[index];
}
const UrdfModel& getModel() const
{
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
}
UrdfModel& getModel()
{
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
{
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
}
@@ -351,4 +344,3 @@ public:
};
#endif

145
examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h
View File

@@ -7,93 +7,90 @@ struct UrdfModel;
///btTransform is a position and 3x3 matrix, as defined in Bullet/src/LinearMath/btTransform
class btTransform;
///UrdfRenderingInterface is a simple rendering interface, mainly for URDF-based robots.
///There is an implementation in
///UrdfRenderingInterface is a simple rendering interface, mainly for URDF-based robots.
///There is an implementation in
///bullet3\examples\SharedMemory\plugins\tinyRendererPlugin\TinyRendererVisualShapeConverter.cpp
struct UrdfRenderingInterface
{
virtual ~UrdfRenderingInterface() {}
{
virtual ~UrdfRenderingInterface() {}
///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) =0;
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) = 0;
///remove a visual shapes, based on the shape unique id (shapeUid)
virtual void removeVisualShape(int collisionObjectUid)=0;
virtual void removeVisualShape(int collisionObjectUid) = 0;
///update the world transform + scaling of the visual shape, using the shapeUid
virtual void syncTransform(int collisionObjectUid, const class btTransform& worldTransform, const class btVector3& localScaling)=0;
virtual void syncTransform(int collisionObjectUid, const class btTransform& worldTransform, const class btVector3& localScaling) = 0;
///return the number of visual shapes, for a particular body unique id
virtual int getNumVisualShapes(int bodyUniqueId)=0;
virtual int getNumVisualShapes(int bodyUniqueId) = 0;
///return the visual shape information, for a particular body unique id and 'shape index'
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData)=0;
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData) = 0;
///change the RGBA color for some visual shape.
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4])=0;
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]) = 0;
///select a given texture for some visual shape.
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId)=0;
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId) = 0;
///pick the up-axis, either Y (1) or Z (2)
virtual void setUpAxis(int axis)=0;
///compute the view matrix based on those parameters
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ)=0;
///clear the render buffer with a particular color.
virtual void clearBuffers(struct TGAColor& clearColor)=0;
///remove all visual shapes.
virtual void resetAll()=0;
///return the frame buffer width and height for the renderer
virtual void getWidthAndHeight(int& width, int& height)=0;
///set the frame buffer width and height for the renderer
virtual void setWidthAndHeight(int width, int height)=0;
///set the light direction, in world coordinates
virtual void setLightDirection(float x, float y, float z)=0;
///set the ambient light color, in world coordinates
virtual void setLightColor(float x, float y, float z)=0;
///set the light distance
virtual void setLightDistance(float dist)=0;
///set the light ambient coefficient
virtual void setLightAmbientCoeff(float ambientCoeff)=0;
///set the light diffuse coefficient
virtual void setLightDiffuseCoeff(float diffuseCoeff)=0;
///set the light specular coefficient
virtual void setLightSpecularCoeff(float specularCoeff)=0;
///enable or disable rendering of shadows
virtual void setShadow(bool hasShadow)=0;
///some undocumented flags for experimentation (todo: document)
virtual void setFlags(int flags)=0;
///provide the image pixels as a part of a stream.
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)=0;
///render an image, using some arbitraty view and projection matrix
virtual void render()=0;
///render an image using the provided view and projection matrix
virtual void render(const float viewMat[16], const float projMat[16])=0;
///load a texture from file, in png or other popular/supported format
virtual int loadTextureFile(const char* filename)=0;
///register a texture using an in-memory pixel buffer of a given width and height
virtual int registerTexture(unsigned char* texels, int width, int height)=0;
virtual void setUpAxis(int axis) = 0;
///compute the view matrix based on those parameters
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ) = 0;
///clear the render buffer with a particular color.
virtual void clearBuffers(struct TGAColor& clearColor) = 0;
///remove all visual shapes.
virtual void resetAll() = 0;
///return the frame buffer width and height for the renderer
virtual void getWidthAndHeight(int& width, int& height) = 0;
///set the frame buffer width and height for the renderer
virtual void setWidthAndHeight(int width, int height) = 0;
///set the light direction, in world coordinates
virtual void setLightDirection(float x, float y, float z) = 0;
///set the ambient light color, in world coordinates
virtual void setLightColor(float x, float y, float z) = 0;
///set the light distance
virtual void setLightDistance(float dist) = 0;
///set the light ambient coefficient
virtual void setLightAmbientCoeff(float ambientCoeff) = 0;
///set the light diffuse coefficient
virtual void setLightDiffuseCoeff(float diffuseCoeff) = 0;
///set the light specular coefficient
virtual void setLightSpecularCoeff(float specularCoeff) = 0;
///enable or disable rendering of shadows
virtual void setShadow(bool hasShadow) = 0;
///some undocumented flags for experimentation (todo: document)
virtual void setFlags(int flags) = 0;
///provide the image pixels as a part of a stream.
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) = 0;
///render an image, using some arbitraty view and projection matrix
virtual void render() = 0;
///render an image using the provided view and projection matrix
virtual void render(const float viewMat[16], const float projMat[16]) = 0;
///load a texture from file, in png or other popular/supported format
virtual int loadTextureFile(const char* filename) = 0;
///register a texture using an in-memory pixel buffer of a given width and height
virtual int registerTexture(unsigned char* texels, int width, int height) = 0;
};
#endif //LINK_VISUAL_SHAPES_CONVERTER_H
#endif //LINK_VISUAL_SHAPES_CONVERTER_H

8
examples/Importers/ImportURDFDemo/urdfLexicalCast.h
View File

@@ -3,15 +3,11 @@
#include <stdlib.h>
template <typename T> T urdfLexicalCast(const char* txt)
template <typename T>
T urdfLexicalCast(const char* txt)
{
double result = atof(txt);
return result;
};
#endif

397
examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
View File

@@ -8,33 +8,29 @@
#include "urdfStringSplit.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators)
void urdfStringSplit(btAlignedObjectArray<std::string> &pieces, const std::string &vector_str, const btAlignedObjectArray<std::string> &separators)
{
assert(separators.size() == 1);
if (separators.size() == 1)
{
assert(separators.size()==1);
if (separators.size()==1)
{
char** strArray = urdfStrSplit(vector_str.c_str(),separators[0].c_str());
int numSubStr = urdfStrArrayLen(strArray);
for (int i=0;i<numSubStr;i++)
pieces.push_back(std::string(strArray[i]));
urdfStrArrayFree(strArray);
}
char **strArray = urdfStrSplit(vector_str.c_str(), separators[0].c_str());
int numSubStr = urdfStrArrayLen(strArray);
for (int i = 0; i < numSubStr; i++)
pieces.push_back(std::string(strArray[i]));
urdfStrArrayFree(strArray);
}
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray)
}
void urdfIsAnyOf(const char *seps, btAlignedObjectArray<std::string> &strArray)
{
int numSeps = strlen(seps);
for (int i = 0; i < numSeps; i++)
{
int numSeps = strlen(seps);
for (int i=0;i<numSeps;i++)
{
char sep2[2] = {0,0};
sep2[0] = seps[i];
strArray.push_back(sep2);
}
char sep2[2] = {0, 0};
sep2[0] = seps[i];
strArray.push_back(sep2);
}
}
/* Append an item to a dynamically allocated array of strings. On failure,
return NULL, in which case the original array is intact. The item
@@ -42,206 +38,219 @@ void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::strin
array. Otherwise, extend the array. Make sure the array is always
NULL-terminated. Input string might not be '\0'-terminated. */
char **urdfStrArrayAppend(char **array, size_t nitems, const char *item,
size_t itemlen)
size_t itemlen)
{
/* Make a dynamic copy of the item. */
char *copy;
if (item == NULL)
copy = NULL;
else {
copy = (char*)malloc(itemlen + 1);
if (copy == NULL)
return NULL;
memcpy(copy, item, itemlen);
copy[itemlen] = '\0';
}
/* Extend array with one element. Except extend it by two elements,
/* Make a dynamic copy of the item. */
char *copy;
if (item == NULL)
copy = NULL;
else
{
copy = (char *)malloc(itemlen + 1);
if (copy == NULL)
return NULL;
memcpy(copy, item, itemlen);
copy[itemlen] = '\0';
}
/* Extend array with one element. Except extend it by two elements,
in case it did not yet exist. This might mean it is a teeny bit
too big, but we don't care. */
array = (char**)realloc(array, (nitems + 2) * sizeof(array[0]));
if (array == NULL) {
free(copy);
return NULL;
}
/* Add copy of item to array, and return it. */
array[nitems] = copy;
array[nitems+1] = NULL;
return array;
}
array = (char **)realloc(array, (nitems + 2) * sizeof(array[0]));
if (array == NULL)
{
free(copy);
return NULL;
}
/* Add copy of item to array, and return it. */
array[nitems] = copy;
array[nitems + 1] = NULL;
return array;
}
/* Free a dynamic array of dynamic strings. */
void urdfStrArrayFree(char **array)
{
if (array == NULL)
return;
for (size_t i = 0; array[i] != NULL; ++i)
free(array[i]);
free(array);
if (array == NULL)
return;
for (size_t i = 0; array[i] != NULL; ++i)
free(array[i]);
free(array);
}
/* Split a string into substrings. Return dynamic array of dynamically
allocated substrings, or NULL if there was an error. Caller is
expected to free the memory, for example with str_array_free. */
char **urdfStrSplit(const char *input, const char *sep)
{
size_t nitems = 0;
char **array = NULL;
const char *start = input;
const char *next = strstr(start, sep);
size_t seplen = strlen(sep);
const char *item;
size_t itemlen;
for (;;) {
next = strstr(start, sep);
if (next == NULL) {
/* Add the remaining string (or empty string, if input ends with
separator. */
char **newstr = urdfStrArrayAppend(array, nitems, start, strlen(start));
if (newstr == NULL) {
urdfStrArrayFree(array);
return NULL;
}
array = newstr;
++nitems;
break;
} else if (next == input) {
/* Input starts with separator. */
item = "";
itemlen = 0;
} else {
item = start;
itemlen = next - item;
}
char **newstr = urdfStrArrayAppend(array, nitems, item, itemlen);
if (newstr == NULL) {
urdfStrArrayFree(array);
return NULL;
}
array = newstr;
++nitems;
start = next + seplen;
}
if (nitems == 0) {
/* Input does not contain separator at all. */
assert(array == NULL);
array = urdfStrArrayAppend(array, nitems, input, strlen(input));
}
return array;
}
size_t nitems = 0;
char **array = NULL;
const char *start = input;
const char *next = strstr(start, sep);
size_t seplen = strlen(sep);
const char *item;
size_t itemlen;
for (;;)
{
next = strstr(start, sep);
if (next == NULL)
{
/* Add the remaining string (or empty string, if input ends with
separator. */
char **newstr = urdfStrArrayAppend(array, nitems, start, strlen(start));
if (newstr == NULL)
{
urdfStrArrayFree(array);
return NULL;
}
array = newstr;
++nitems;
break;
}
else if (next == input)
{
/* Input starts with separator. */
item = "";
itemlen = 0;
}
else
{
item = start;
itemlen = next - item;
}
char **newstr = urdfStrArrayAppend(array, nitems, item, itemlen);
if (newstr == NULL)
{
urdfStrArrayFree(array);
return NULL;
}
array = newstr;
++nitems;
start = next + seplen;
}
if (nitems == 0)
{
/* Input does not contain separator at all. */
assert(array == NULL);
array = urdfStrArrayAppend(array, nitems, input, strlen(input));
}
return array;
}
/* Return length of a NULL-delimited array of strings. */
size_t urdfStrArrayLen(char **array)
{
size_t len;
for (len = 0; array[len] != NULL; ++len)
continue;
return len;
size_t len;
for (len = 0; array[len] != NULL; ++len)
continue;
return len;
}
#ifdef UNIT_TEST_STRING_SPLIT
#define MAX_OUTPUT 20
int main(void)
{
struct {
const char *input;
const char *sep;
char *output[MAX_OUTPUT];
} tab[] = {
/* Input is empty string. Output should be a list with an empty
struct
{
const char *input;
const char *sep;
char *output[MAX_OUTPUT];
} tab[] = {
/* Input is empty string. Output should be a list with an empty
string. */
{
"",
"and",
{
"",
NULL,
},
},
/* Input is exactly the separator. Output should be two empty
{
"",
"and",
{
"",
NULL,
},
},
/* Input is exactly the separator. Output should be two empty
strings. */
{
"and",
"and",
{
"",
"",
NULL,
},
},
/* Input is non-empty, but does not have separator. Output should
{
"and",
"and",
{
"",
"",
NULL,
},
},
/* Input is non-empty, but does not have separator. Output should
be the same string. */
{
"foo",
"and",
{
"foo",
NULL,
},
},
/* Input is non-empty, and does have separator. */
{
"foo bar 1 and foo bar 2",
" and ",
{
"foo bar 1",
"foo bar 2",
NULL,
},
},
};
const int tab_len = sizeof(tab) / sizeof(tab[0]);
bool errors;
errors = false;
for (int i = 0; i < tab_len; ++i) {
printf("test %d\n", i);
char **output = str_split(tab[i].input, tab[i].sep);
if (output == NULL) {
fprintf(stderr, "output is NULL\n");
errors = true;
break;
}
size_t num_output = str_array_len(output);
printf("num_output %lu\n", (unsigned long) num_output);
size_t num_correct = str_array_len(tab[i].output);
if (num_output != num_correct) {
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
(unsigned long) num_output, (unsigned long) num_correct);
errors = true;
} else {
for (size_t j = 0; j < num_output; ++j) {
if (strcmp(tab[i].output[j], output[j]) != 0) {
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
(unsigned long) j, output[j], tab[i].output[j]);
errors = true;
break;
}
}
}
str_array_free(output);
printf("\n");
}
if (errors)
return EXIT_FAILURE;
return 0;
{
"foo",
"and",
{
"foo",
NULL,
},
},
/* Input is non-empty, and does have separator. */
{
"foo bar 1 and foo bar 2",
" and ",
{
"foo bar 1",
"foo bar 2",
NULL,
},
},
};
const int tab_len = sizeof(tab) / sizeof(tab[0]);
bool errors;
errors = false;
for (int i = 0; i < tab_len; ++i)
{
printf("test %d\n", i);
char **output = str_split(tab[i].input, tab[i].sep);
if (output == NULL)
{
fprintf(stderr, "output is NULL\n");
errors = true;
break;
}
size_t num_output = str_array_len(output);
printf("num_output %lu\n", (unsigned long)num_output);
size_t num_correct = str_array_len(tab[i].output);
if (num_output != num_correct)
{
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
(unsigned long)num_output, (unsigned long)num_correct);
errors = true;
}
else
{
for (size_t j = 0; j < num_output; ++j)
{
if (strcmp(tab[i].output[j], output[j]) != 0)
{
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
(unsigned long)j, output[j], tab[i].output[j]);
errors = true;
break;
}
}
}
str_array_free(output);
printf("\n");
}
if (errors)
return EXIT_FAILURE;
return 0;
}
#endif//UNIT_TEST_STRING_SPLIT
#endif //UNIT_TEST_STRING_SPLIT

25
examples/Importers/ImportURDFDemo/urdfStringSplit.h
View File

@@ -8,33 +8,32 @@
#include <string>
#include "LinearMath/btAlignedObjectArray.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
void urdfStringSplit(btAlignedObjectArray<std::string>& pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
#endif
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
/* Split a string into substrings. Return dynamic array of dynamically
/* Split a string into substrings. Return dynamic array of dynamically
allocated substrings, or NULL if there was an error. Caller is
expected to free the memory, for example with str_array_free. */
char** urdfStrSplit(const char* input, const char* sep);
char** urdfStrSplit(const char* input, const char* sep);
/* Free a dynamic array of dynamic strings. */
void urdfStrArrayFree(char** array);
/* Free a dynamic array of dynamic strings. */
void urdfStrArrayFree(char** array);
/* Return length of a NULL-delimited array of strings. */
size_t urdfStrArrayLen(char** array);
/* Return length of a NULL-delimited array of strings. */
size_t urdfStrArrayLen(char** array);
#ifdef __cplusplus
}
#endif
#endif //STRING_SPLIT_H
#endif //STRING_SPLIT_H

46
examples/Importers/ImportURDFDemo/urdf_samples.h
View File

@@ -3,29 +3,28 @@
#define MSTRINGIFY(A) #A
const char* urdf_char2 = MSTRINGIFY(
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
</joint>
</robot>);
<robot name = "test_robot">
<link name = "link1" />
<link name = "link2" />
<link name = "link3" />
<link name = "link4" />
<joint name = "joint1" type = "continuous">
<parent link = "link1" />
<child link = "link2" />
</ joint>
<joint name = "joint2" type = "continuous">
<parent link = "link1" />
<child link = "link3" />
</ joint>
<joint name = "joint3" type = "continuous">
<parent link = "link3" />
<child link = "link4" />
</ joint>
</ robot>);
const char* urdf_char1 = MSTRINGIFY(
<?xml version="1.0"?>
@@ -821,5 +820,4 @@ const char* urdf_char = MSTRINGIFY(
);
#endif //URDF_SAMPLES_H
#endif //URDF_SAMPLES_H