add initial examples, replacing the 'Demos/Demos3'. Will make it work cross-platform, OpenGL3/OpenGL2 and add more examples to it.
This commit is contained in:
erwincoumans
207
examples/Importers/ImportBsp/BspConverter.cpp
Normal file
207
examples/Importers/ImportBsp/BspConverter.cpp
Normal file
@@ -0,0 +1,207 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "BspConverter.h"
|
||||
#include "BspLoader.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btGeometryUtil.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
|
||||
{
|
||||
{
|
||||
|
||||
float playstartf[3] = {0,0,100};
|
||||
|
||||
if (bspLoader.findVectorByName(&playstartf[0],"info_player_start"))
|
||||
{
|
||||
printf("found playerstart\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
if (bspLoader.findVectorByName(&playstartf[0],"info_player_deathmatch"))
|
||||
{
|
||||
printf("found deatchmatch start\n");
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 playerStart (playstartf[0],playstartf[1],playstartf[2]);
|
||||
|
||||
|
||||
playerStart[2] += 20.f; //start a bit higher
|
||||
|
||||
playerStart *= scaling;
|
||||
|
||||
|
||||
|
||||
//progressBegin("Loading bsp");
|
||||
|
||||
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)) );
|
||||
|
||||
bool isValidBrush = false;
|
||||
|
||||
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];
|
||||
|
||||
BSPBrush& brush = bspLoader.m_dbrushes[brushid];
|
||||
if (brush.shaderNum!=-1)
|
||||
{
|
||||
if (bspLoader.m_dshaders[ brush.shaderNum ].contentFlags & BSPCONTENTS_SOLID)
|
||||
{
|
||||
brush.shaderNum = -1;
|
||||
|
||||
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 = false;
|
||||
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++)
|
||||
{
|
||||
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);
|
||||
|
||||
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)
|
||||
{
|
||||
if (bspLoader.getVectorForKey( targetentity , "origin",&targetLocation[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))
|
||||
{
|
||||
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];
|
||||
{
|
||||
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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("unsupported trigger_push model, md3 ?\n");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif //USE_ENTITIES
|
||||
|
||||
|
||||
|
||||
//progressEnd();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
39
examples/Importers/ImportBsp/BspConverter.h
Normal file
39
examples/Importers/ImportBsp/BspConverter.h
Normal file
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef BSP_CONVERTER_H
|
||||
#define BSP_CONVERTER_H
|
||||
|
||||
class BspLoader;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
///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;
|
||||
|
||||
};
|
||||
|
||||
#endif //BSP_CONVERTER_H
|
||||
|
||||
730
examples/Importers/ImportBsp/BspLoader.cpp
Normal file
730
examples/Importers/ImportBsp/BspLoader.cpp
Normal file
@@ -0,0 +1,730 @@
|
||||
/*
|
||||
===========================================================================
|
||||
Copyright (C) 1999-2005 Id Software, Inc.
|
||||
|
||||
This file is part of Quake III Arena source code.
|
||||
|
||||
Quake III Arena source code is free software; you can redistribute it
|
||||
and/or modify it under the terms of the GNU bteral Public License as
|
||||
published by the Free Software Foundation; either version 2 of the License,
|
||||
or (at your option) any later version.
|
||||
|
||||
Quake III Arena source code is distributed in the hope that it will be
|
||||
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU bteral Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU bteral Public License
|
||||
along with Foobar; if not, write to the Free Software
|
||||
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;
|
||||
} BSPScript;
|
||||
|
||||
#define MAX_INCLUDES 8
|
||||
BSPScript scriptstack[MAX_INCLUDES];
|
||||
BSPScript *script;
|
||||
int scriptline;
|
||||
|
||||
char token[BSPMAXTOKEN];
|
||||
bool endofscript;
|
||||
bool tokenready; // only true if UnGetToken was just called
|
||||
|
||||
//
|
||||
//loadBSPFile
|
||||
//
|
||||
|
||||
int extrasize = 100;
|
||||
|
||||
BspLoader::BspLoader()
|
||||
:m_num_entities(0)
|
||||
{
|
||||
m_Endianness = getMachineEndianness();
|
||||
if (m_Endianness == BSP_BIG_ENDIAN)
|
||||
{
|
||||
printf("Machine is BIG_ENDIAN\n");
|
||||
} else
|
||||
{
|
||||
printf("Machine is Little Endian\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool BspLoader::loadBSPFile( void* memoryBuffer) {
|
||||
|
||||
BSPHeader *header = (BSPHeader*) memoryBuffer;
|
||||
|
||||
// load the file header
|
||||
if (header)
|
||||
{
|
||||
// swap the 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) );
|
||||
|
||||
length = (header->lumps[LUMP_MODELS].filelen) / 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) );
|
||||
|
||||
length = (header->lumps[BSPLUMP_LEAFS].filelen) / 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) );
|
||||
|
||||
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]) );
|
||||
|
||||
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) );
|
||||
|
||||
|
||||
length = (header->lumps[LUMP_BRUSHSIDES].filelen) / 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) );
|
||||
|
||||
|
||||
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]) );
|
||||
|
||||
length = (header->lumps[LUMP_VISIBILITY].filelen) / 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 );
|
||||
|
||||
length = (header->lumps[BSPLUMP_ENTITIES].filelen) / 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 );
|
||||
|
||||
// swap everything
|
||||
swapBSPFile();
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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) ) {
|
||||
return ep->value;
|
||||
}
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
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);
|
||||
if (strcmp(k, ""))
|
||||
{
|
||||
sscanf (k, "%f %f %f", &vec[0], &vec[1], &vec[2]);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
==============
|
||||
parseFromMemory
|
||||
==============
|
||||
*/
|
||||
void BspLoader::parseFromMemory (char *buffer, int size)
|
||||
{
|
||||
script = scriptstack;
|
||||
script++;
|
||||
if (script == &scriptstack[MAX_INCLUDES])
|
||||
{
|
||||
//printf("script file exceeded MAX_INCLUDES");
|
||||
}
|
||||
strcpy (script->filename, "memory buffer" );
|
||||
|
||||
script->buffer = buffer;
|
||||
script->line = 1;
|
||||
script->script_p = script->buffer;
|
||||
script->end_p = script->buffer + size;
|
||||
|
||||
endofscript = false;
|
||||
tokenready = false;
|
||||
}
|
||||
|
||||
|
||||
bool BspLoader::isEndOfScript (bool crossline)
|
||||
{
|
||||
if (!crossline)
|
||||
//printf("Line %i is incomplete\n",scriptline);
|
||||
|
||||
if (!strcmp (script->filename, "memory buffer"))
|
||||
{
|
||||
endofscript = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
//free (script->buffer);
|
||||
if (script == scriptstack+1)
|
||||
{
|
||||
endofscript = true;
|
||||
return false;
|
||||
}
|
||||
script--;
|
||||
scriptline = script->line;
|
||||
//printf ("returning to %s\n", script->filename);
|
||||
return getToken (crossline);
|
||||
}
|
||||
|
||||
/*
|
||||
|
||||
==============
|
||||
getToken
|
||||
==============
|
||||
*/
|
||||
bool BspLoader::getToken (bool crossline)
|
||||
{
|
||||
char *token_p;
|
||||
|
||||
if (tokenready) // is a token allready waiting?
|
||||
{
|
||||
tokenready = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (script->script_p >= script->end_p)
|
||||
return isEndOfScript (crossline);
|
||||
|
||||
//
|
||||
// skip space
|
||||
//
|
||||
skipspace:
|
||||
while (*script->script_p <= 32)
|
||||
{
|
||||
if (script->script_p >= script->end_p)
|
||||
return isEndOfScript (crossline);
|
||||
if (*script->script_p++ == '\n')
|
||||
{
|
||||
if (!crossline)
|
||||
{
|
||||
//printf("Line %i is incomplete\n",scriptline);
|
||||
}
|
||||
scriptline = script->line++;
|
||||
}
|
||||
}
|
||||
|
||||
if (script->script_p >= script->end_p)
|
||||
return isEndOfScript (crossline);
|
||||
|
||||
// ; # // comments
|
||||
if (*script->script_p == ';' || *script->script_p == '#'
|
||||
|| ( script->script_p[0] == '/' && script->script_p[1] == '/') )
|
||||
{
|
||||
if (!crossline)
|
||||
{
|
||||
//printf("Line %i is incomplete\n",scriptline);
|
||||
}
|
||||
while (*script->script_p++ != '\n')
|
||||
if (script->script_p >= script->end_p)
|
||||
return isEndOfScript (crossline);
|
||||
scriptline = script->line++;
|
||||
goto skipspace;
|
||||
}
|
||||
|
||||
// /* */ comments
|
||||
if (script->script_p[0] == '/' && script->script_p[1] == '*')
|
||||
{
|
||||
if (!crossline)
|
||||
{
|
||||
//printf("Line %i is incomplete\n",scriptline);
|
||||
}
|
||||
script->script_p+=2;
|
||||
while (script->script_p[0] != '*' && script->script_p[1] != '/')
|
||||
{
|
||||
if ( *script->script_p == '\n' ) {
|
||||
scriptline = script->line++;
|
||||
}
|
||||
script->script_p++;
|
||||
if (script->script_p >= script->end_p)
|
||||
return isEndOfScript (crossline);
|
||||
}
|
||||
script->script_p += 2;
|
||||
goto skipspace;
|
||||
}
|
||||
|
||||
//
|
||||
// copy token
|
||||
//
|
||||
token_p = token;
|
||||
|
||||
if (*script->script_p == '"')
|
||||
{
|
||||
// quoted token
|
||||
script->script_p++;
|
||||
while (*script->script_p != '"')
|
||||
{
|
||||
*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);
|
||||
}
|
||||
}
|
||||
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])
|
||||
{
|
||||
//printf ("Token too large on line %i\n",scriptline);
|
||||
}
|
||||
}
|
||||
|
||||
*token_p = 0;
|
||||
|
||||
if (!strcmp (token, "$include"))
|
||||
{
|
||||
//getToken (false);
|
||||
//AddScriptToStack (token);
|
||||
return false;//getToken (crossline);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
char *BspLoader::copystring(const char *s)
|
||||
{
|
||||
char *b;
|
||||
b = (char*) malloc( strlen(s)+1);
|
||||
strcpy (b, s);
|
||||
return b;
|
||||
}
|
||||
|
||||
void BspLoader::stripTrailing( char *e ) {
|
||||
char *s;
|
||||
|
||||
s = e + strlen(e)-1;
|
||||
while (s >= e && *s <= 32)
|
||||
{
|
||||
*s = 0;
|
||||
s--;
|
||||
}
|
||||
}
|
||||
/*
|
||||
=================
|
||||
parseEpair
|
||||
=================
|
||||
*/
|
||||
BSPKeyValuePair *BspLoader::parseEpair( void ) {
|
||||
BSPKeyValuePair *e;
|
||||
|
||||
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 ) {
|
||||
|
||||
//printf ("ParseEpar: token too long");
|
||||
}
|
||||
e->value = copystring( token );
|
||||
|
||||
// strip trailing spaces that sometimes get accidentally
|
||||
// added in the editor
|
||||
stripTrailing( e->key );
|
||||
stripTrailing( e->value );
|
||||
|
||||
return e;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
================
|
||||
parseEntity
|
||||
================
|
||||
*/
|
||||
bool BspLoader::parseEntity( void ) {
|
||||
BSPKeyValuePair *e;
|
||||
BSPEntity *mapent;
|
||||
|
||||
if ( !getToken (true) ) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if ( strcmp (token, "{") ) {
|
||||
|
||||
//printf ("parseEntity: { not found");
|
||||
}
|
||||
|
||||
BSPEntity bla;
|
||||
bla.brushes = 0;
|
||||
bla.epairs = 0;
|
||||
bla.firstDrawSurf = 0;
|
||||
bla.origin[0] = 0.f;
|
||||
bla.origin[1] = 0.f;
|
||||
bla.origin[2] = 0.f;
|
||||
bla.patches = 0;
|
||||
|
||||
m_entities.push_back(bla);
|
||||
mapent = &m_entities[m_entities.size()-1];
|
||||
m_num_entities++;
|
||||
|
||||
do {
|
||||
if ( !getToken (true) ) {
|
||||
//printf("parseEntity: EOF without closing brace");
|
||||
}
|
||||
if ( !strcmp (token, "}") ) {
|
||||
break;
|
||||
}
|
||||
e = (struct BSPPair*)parseEpair ();
|
||||
e->next = mapent->epairs;
|
||||
mapent->epairs = e;
|
||||
} while (1);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
================
|
||||
parseEntities
|
||||
|
||||
Parses the dentdata string into entities
|
||||
================
|
||||
*/
|
||||
void BspLoader::parseEntities( void ) {
|
||||
m_num_entities = 0;
|
||||
m_entities.clear();
|
||||
|
||||
parseFromMemory( &m_dentdata[0], m_entdatasize );
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
short BspLoader::isLittleShort (short l)
|
||||
{
|
||||
if (machineEndianness() == BSP_BIG_ENDIAN)
|
||||
{
|
||||
unsigned char b1,b2;
|
||||
|
||||
b1 = l&255;
|
||||
b2 = (l>>8)&255;
|
||||
|
||||
return (b1<<8) + b2;
|
||||
}
|
||||
//little endian
|
||||
return 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;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
int BspLoader::isLittleLong (int l)
|
||||
{
|
||||
if (machineEndianness() == BSP_BIG_ENDIAN)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
//little endian
|
||||
return l;
|
||||
|
||||
}
|
||||
|
||||
int BspLoader::isBigLong (int l)
|
||||
{
|
||||
if (machineEndianness() == BSP_BIG_ENDIAN)
|
||||
{
|
||||
return l;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
float BspLoader::isLittleFloat (float l)
|
||||
{
|
||||
if (machineEndianness() == BSP_BIG_ENDIAN)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
//little endian
|
||||
return l;
|
||||
}
|
||||
|
||||
float BspLoader::isBigFloat (float l)
|
||||
{
|
||||
if (machineEndianness() == BSP_BIG_ENDIAN)
|
||||
{
|
||||
return l;
|
||||
}
|
||||
//little endian
|
||||
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;
|
||||
|
||||
sizeOfBlock >>= 2;
|
||||
for ( i = 0 ; i < sizeOfBlock ; i++ ) {
|
||||
block[i] = isLittleLong( block[i] );
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// copyLump
|
||||
//
|
||||
|
||||
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 );
|
||||
|
||||
return length / size;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
// swapBSPFile
|
||||
//
|
||||
|
||||
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 );
|
||||
}
|
||||
|
||||
// planes
|
||||
swapBlock( (int *)&m_dplanes[0], m_numplanes * sizeof( m_dplanes[0] ) );
|
||||
|
||||
// nodes
|
||||
swapBlock( (int *)&m_dnodes[0], m_numnodes * sizeof( m_dnodes[0] ) );
|
||||
|
||||
// leafs
|
||||
swapBlock( (int *)&m_dleafs[0], m_numleafs * sizeof( m_dleafs[0] ) );
|
||||
|
||||
// leaffaces
|
||||
swapBlock( (int *)&m_dleafsurfaces[0], m_numleafsurfaces * sizeof( m_dleafsurfaces[0] ) );
|
||||
|
||||
// leafbrushes
|
||||
swapBlock( (int *)&m_dleafbrushes[0], m_numleafbrushes * sizeof( m_dleafbrushes[0] ) );
|
||||
|
||||
// brushes
|
||||
swapBlock( (int *)&m_dbrushes[0], m_numbrushes * sizeof( m_dbrushes[0] ) );
|
||||
|
||||
// brushsides
|
||||
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] );
|
||||
|
||||
|
||||
// drawindexes
|
||||
swapBlock( (int *)&m_drawIndexes[0], m_numDrawIndexes * sizeof( m_drawIndexes[0] ) );
|
||||
|
||||
// drawsurfs
|
||||
swapBlock( (int *)&m_drawSurfaces[0], m_numDrawSurfaces * sizeof( m_drawSurfaces[0] ) );
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
if (found)
|
||||
{
|
||||
outvec[0] = origin[0];
|
||||
outvec[1] = origin[1];
|
||||
outvec[2] = origin[2];
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
|
||||
|
||||
const BSPEntity * BspLoader::getEntityByValue( const char* name, const char* value)
|
||||
{
|
||||
const BSPEntity* entity = NULL;
|
||||
|
||||
for ( int i = 1; i < m_num_entities; i++ ) {
|
||||
|
||||
const BSPEntity& ent = m_entities[i];
|
||||
|
||||
const char* cl = getValueForKey (&m_entities[i], name);
|
||||
if ( !strcmp( cl, value ) ) {
|
||||
entity = &ent;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return entity;
|
||||
}
|
||||
|
||||
295
examples/Importers/ImportBsp/BspLoader.h
Normal file
295
examples/Importers/ImportBsp/BspLoader.h
Normal file
@@ -0,0 +1,295 @@
|
||||
/*
|
||||
===========================================================================
|
||||
Copyright (C) 1999-2005 Id Software, Inc.
|
||||
|
||||
This file is part of Quake III Arena source code.
|
||||
|
||||
Quake III Arena source code is free software; you can redistribute it
|
||||
and/or modify it under the terms of the GNU bteral Public License as
|
||||
published by the Free Software Foundation; either version 2 of the License,
|
||||
or (at your option) any later version.
|
||||
|
||||
Quake III Arena source code is distributed in the hope that it will be
|
||||
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU bteral Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU bteral Public License
|
||||
along with Foobar; if not, write to the Free Software
|
||||
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
|
||||
|
||||
|
||||
|
||||
typedef struct {
|
||||
int fileofs, filelen;
|
||||
} BSPLump;
|
||||
|
||||
typedef float BSPVector3[3];
|
||||
|
||||
typedef struct {
|
||||
int ident;
|
||||
int version;
|
||||
|
||||
BSPLump lumps[HEADER_LUMPS];
|
||||
} BSPHeader;
|
||||
|
||||
|
||||
typedef struct {
|
||||
float mins[3], maxs[3];
|
||||
int firstSurface, numSurfaces;
|
||||
int firstBrush, numBrushes;
|
||||
} BSPModel;
|
||||
|
||||
typedef struct {
|
||||
char shader[MAX_QPATH];
|
||||
int surfaceFlags;
|
||||
int contentFlags;
|
||||
} BSPShader;
|
||||
|
||||
typedef struct {
|
||||
float normal[3];
|
||||
float dist;
|
||||
} BSPPlane;
|
||||
|
||||
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;
|
||||
} BSPLeaf;
|
||||
|
||||
typedef struct {
|
||||
int planeNum;
|
||||
int shaderNum;
|
||||
} BSPBrushSide;
|
||||
|
||||
typedef struct {
|
||||
int firstSide;
|
||||
int numSides;
|
||||
int shaderNum;
|
||||
} BSPBrush;
|
||||
|
||||
|
||||
|
||||
|
||||
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;
|
||||
} BSPEntity;
|
||||
|
||||
typedef enum {
|
||||
MST_BAD,
|
||||
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;
|
||||
} 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:
|
||||
|
||||
BspLoader();
|
||||
|
||||
bool loadBSPFile( void* memoryBuffer);
|
||||
|
||||
const char* getValueForKey( const BSPEntity *ent, const char *key ) const;
|
||||
|
||||
bool getVectorForKey( const BSPEntity *ent, const char *key, BSPVector3 vec );
|
||||
|
||||
float getFloatForKey( const BSPEntity *ent, const char *key );
|
||||
|
||||
void parseEntities( void );
|
||||
|
||||
bool findVectorByName(float* outvec,const char* name);
|
||||
|
||||
const BSPEntity * getEntityByValue( const char* name, const char* value);
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
void parseFromMemory (char *buffer, int size);
|
||||
|
||||
|
||||
|
||||
bool isEndOfScript (bool crossline);
|
||||
|
||||
bool getToken (bool crossline);
|
||||
|
||||
char *copystring(const char *s);
|
||||
|
||||
void stripTrailing( char *e );
|
||||
|
||||
BSPKeyValuePair * parseEpair( void );
|
||||
|
||||
bool parseEntity( void );
|
||||
|
||||
short isLittleShort (short l);
|
||||
int isLittleLong (int l);
|
||||
float isLittleFloat (float l);
|
||||
|
||||
int isBigLong (int l);
|
||||
short isBigShort (short l);
|
||||
float isBigFloat (float l);
|
||||
|
||||
void swapBlock( int *block, int sizeOfBlock );
|
||||
|
||||
int copyLump( BSPHeader *header, int lump, void *dest, int size );
|
||||
|
||||
void swapBSPFile( void );
|
||||
|
||||
|
||||
|
||||
|
||||
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_entdatasize;
|
||||
btAlignedObjectArray<char> m_dentdata;
|
||||
|
||||
int m_numleafs;
|
||||
btAlignedObjectArray<BSPLeaf> m_dleafs;
|
||||
|
||||
int m_numplanes;
|
||||
btAlignedObjectArray<BSPPlane> m_dplanes;
|
||||
|
||||
int m_numnodes;
|
||||
btAlignedObjectArray<BSPNode> m_dnodes;
|
||||
|
||||
int m_numleafsurfaces;
|
||||
btAlignedObjectArray<int> m_dleafsurfaces;
|
||||
|
||||
int m_numleafbrushes;
|
||||
btAlignedObjectArray<int> m_dleafbrushes;
|
||||
|
||||
int m_numbrushes;
|
||||
btAlignedObjectArray<BSPBrush> m_dbrushes;
|
||||
|
||||
int m_numbrushsides;
|
||||
btAlignedObjectArray<BSPBrushSide> m_dbrushsides;
|
||||
|
||||
int m_numLightBytes;
|
||||
btAlignedObjectArray<unsigned char> m_lightBytes;
|
||||
|
||||
int m_numGridPoints;
|
||||
btAlignedObjectArray<unsigned char> m_gridData;
|
||||
|
||||
int m_numVisBytes;
|
||||
btAlignedObjectArray<unsigned char> m_visBytes;
|
||||
|
||||
|
||||
int m_numDrawIndexes;
|
||||
btAlignedObjectArray<int> m_drawIndexes;
|
||||
|
||||
int m_numDrawSurfaces;
|
||||
btAlignedObjectArray<BSPSurface> m_drawSurfaces;
|
||||
|
||||
enum
|
||||
{
|
||||
BSP_LITTLE_ENDIAN = 0,
|
||||
BSP_BIG_ENDIAN = 1
|
||||
};
|
||||
|
||||
//returns machines big endian / little endian
|
||||
//
|
||||
int getMachineEndianness();
|
||||
|
||||
inline int machineEndianness()
|
||||
{
|
||||
return m_Endianness;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BSP_LOADER_H
|
||||
295
examples/Importers/ImportBsp/ImportBspExample.cpp
Normal file
295
examples/Importers/ImportBsp/ImportBspExample.cpp
Normal file
@@ -0,0 +1,295 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "ImportBspExample.h"
|
||||
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "LinearMath/btIDebugDraw.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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#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:
|
||||
|
||||
//keep the collision shapes, for deletion/cleanup
|
||||
|
||||
|
||||
BspDemo(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~BspDemo();
|
||||
|
||||
virtual void initPhysics();
|
||||
|
||||
void initPhysics(const char* bspfilename);
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#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)
|
||||
{
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
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);
|
||||
|
||||
//btRigidBody* body = m_demoApp->localCreateRigidBody(mass, startTransform,shape);
|
||||
m_demoApp->createRigidBody(mass, startTransform,shape);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
BspDemo::~BspDemo()
|
||||
{
|
||||
exitPhysics(); //will delete all default data
|
||||
}
|
||||
|
||||
void BspDemo::initPhysics()
|
||||
{
|
||||
const char* bspfilename = "BspDemo.bsp";
|
||||
|
||||
initPhysics(bspfilename);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void BspDemo::initPhysics(const char* bspfilename)
|
||||
{
|
||||
|
||||
int cameraUpAxis =2;
|
||||
btVector3 grav(0,0,0);
|
||||
grav[cameraUpAxis] = -10;
|
||||
m_guiHelper->setUpAxis(cameraUpAxis);
|
||||
|
||||
//_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));
|
||||
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
|
||||
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->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;
|
||||
}
|
||||
|
||||
|
||||
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 */
|
||||
printf("Error: cannot get filesize from %s\n", bspfilename);
|
||||
} else
|
||||
{
|
||||
//how to detect file size?
|
||||
memoryBuffer = malloc(size+1);
|
||||
fread(memoryBuffer,1,size,file);
|
||||
bspLoader.loadBSPFile( memoryBuffer);
|
||||
|
||||
BspToBulletConverter bsp2bullet(this);
|
||||
float bspScaling = 0.1f;
|
||||
bsp2bullet.convertBsp(bspLoader,bspScaling);
|
||||
|
||||
}
|
||||
fclose(file);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//some code that de-mangles the windows filename passed in as argument
|
||||
char cleaned_filename[512];
|
||||
char* getLastFileName()
|
||||
{
|
||||
return cleaned_filename;
|
||||
}
|
||||
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;
|
||||
char* out = cleaned_filename;
|
||||
*out = '\0';
|
||||
// If the first character is a ", skip it (filenames with spaces in them are quoted)
|
||||
if(*in == '\"')
|
||||
{
|
||||
in++;
|
||||
}
|
||||
int 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'))
|
||||
break;
|
||||
|
||||
// If we hit a null or a quote, stop copying. This will get just the first filename.
|
||||
if(*in == '\0' || *in == '\"')
|
||||
break;
|
||||
// Copy while swapping backslashes for forward ones
|
||||
if(*in == '\\')
|
||||
{
|
||||
*out = '/';
|
||||
}
|
||||
else
|
||||
{
|
||||
*out = *in;
|
||||
}
|
||||
in++;
|
||||
out++;
|
||||
}
|
||||
*(out++) = '.';
|
||||
*(out++) = 'b';
|
||||
*(out++) = 's';
|
||||
*(out++) = 'p';
|
||||
*(out++) = 0;
|
||||
|
||||
return cleaned_filename;
|
||||
}
|
||||
|
||||
|
||||
struct ExampleInterface* ImportBspCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
|
||||
{
|
||||
BspDemo* demo = new BspDemo(helper);
|
||||
|
||||
demo->initPhysics("BspDemo.bsp");
|
||||
return demo;
|
||||
|
||||
}
|
||||
/*
|
||||
static DemoApplication* Create()
|
||||
{
|
||||
BspDemo* demo = new BspDemo;
|
||||
demo->myinit();
|
||||
demo->initPhysics("BspDemo.bsp");
|
||||
return demo;
|
||||
}
|
||||
*/
|
||||
23
examples/Importers/ImportBsp/ImportBspExample.h
Normal file
23
examples/Importers/ImportBsp/ImportBspExample.h
Normal file
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef BSP_DEMO_H
|
||||
#define BSP_DEMO_H
|
||||
|
||||
struct ExampleInterface* ImportBspCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
|
||||
|
||||
|
||||
#endif //BSP_DEMO_H
|
||||
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
Bullet Collision Detection and Physics Library http://bulletphysics.org
|
||||
This file is Copyright (c) 2014 Google Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
//original author: Erwin Coumans
|
||||
*/
|
||||
|
||||
#ifndef COLLADA_GRAPHICS_INSTANCE_H
|
||||
#define COLLADA_GRAPHICS_INSTANCE_H
|
||||
|
||||
#include "btMatrix4x4.h"
|
||||
|
||||
struct ColladaGraphicsInstance
|
||||
{
|
||||
ColladaGraphicsInstance()
|
||||
:m_shapeIndex(-1)
|
||||
{
|
||||
m_worldTransform.setIdentity();
|
||||
}
|
||||
btMatrix4x4 m_worldTransform;
|
||||
int m_shapeIndex;//could be index into array of GLInstanceGraphicsShape
|
||||
float m_color[4];
|
||||
};
|
||||
|
||||
#endif //COLLADA_GRAPHICS_INSTANCE_H
|
||||
203
examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp
Normal file
203
examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp
Normal file
@@ -0,0 +1,203 @@
|
||||
/*
|
||||
Bullet Collision Detection and Physics Library http://bulletphysics.org
|
||||
This file is Copyright (c) 2014 Google Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
//original author: Erwin Coumans
|
||||
*/
|
||||
|
||||
|
||||
#include "ImportColladaSetup.h"
|
||||
#include <vector>
|
||||
#include "../OpenGLWindow/GLInstancingRenderer.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "../OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "LoadMeshFromCollada.h"
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
|
||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||
|
||||
|
||||
|
||||
class ImportColladaSetup : public CommonRigidBodyBase
|
||||
{
|
||||
|
||||
public:
|
||||
ImportColladaSetup(struct GUIHelperInterface* helper);
|
||||
virtual ~ImportColladaSetup();
|
||||
|
||||
virtual void initPhysics();
|
||||
};
|
||||
|
||||
ImportColladaSetup::ImportColladaSetup(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
ImportColladaSetup::~ImportColladaSetup()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
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;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void ImportColladaSetup::initPhysics()
|
||||
{
|
||||
int upAxis=1;
|
||||
m_guiHelper->setUpAxis(upAxis);
|
||||
this->createEmptyDynamicsWorld();
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
|
||||
|
||||
static int fileIndex = 0;
|
||||
|
||||
const char* fileNames[] = {
|
||||
"duck.dae",
|
||||
"seymourplane_triangulate.dae",
|
||||
};
|
||||
const char* fileName = fileNames[fileIndex];
|
||||
int numFiles = sizeof(fileNames)/sizeof(const char*);
|
||||
|
||||
char relativeFileName[1024];
|
||||
|
||||
b3FileUtils f;
|
||||
if (!f.findFile(fileName,relativeFileName,1024))
|
||||
return;
|
||||
|
||||
btVector3 shift(0,0,0);
|
||||
btVector3 scaling(1,1,1);
|
||||
// int index=10;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
|
||||
float unitMeterScaling(1);
|
||||
btTransform upAxisTrans;
|
||||
upAxisTrans.setIdentity();
|
||||
|
||||
btVector3 color(0,0,1);
|
||||
|
||||
#ifdef COMPARE_WITH_ASSIMP
|
||||
static int useAssimp = 0;
|
||||
if (useAssimp)
|
||||
{
|
||||
|
||||
LoadMeshFromColladaAssimp(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling);
|
||||
fileIndex++;
|
||||
if (fileIndex>=numFiles)
|
||||
{
|
||||
fileIndex = 0;
|
||||
}
|
||||
color.setValue(1,0,0);
|
||||
}
|
||||
else
|
||||
{
|
||||
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling);
|
||||
|
||||
}
|
||||
useAssimp=1-useAssimp;
|
||||
#else
|
||||
fileIndex++;
|
||||
if (fileIndex>=numFiles)
|
||||
{
|
||||
fileIndex = 0;
|
||||
}
|
||||
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++)
|
||||
{
|
||||
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]);
|
||||
|
||||
//sort the visualShapeInstances, then iterate etc
|
||||
//void LoadMeshFromCollada(const char* relativeFileName,
|
||||
//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++)
|
||||
{
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
}
|
||||
|
||||
//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;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
for(int v=0;v<verts.size();v++)
|
||||
{
|
||||
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class ExampleInterface* ImportColladaCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
|
||||
{
|
||||
return new ImportColladaSetup(helper);
|
||||
}
|
||||
25
examples/Importers/ImportColladaDemo/ImportColladaSetup.h
Normal file
25
examples/Importers/ImportColladaDemo/ImportColladaSetup.h
Normal file
@@ -0,0 +1,25 @@
|
||||
/*
|
||||
Bullet Collision Detection and Physics Library http://bulletphysics.org
|
||||
This file is Copyright (c) 2014 Google Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
//original author: Erwin Coumans
|
||||
*/
|
||||
|
||||
|
||||
#ifndef IMPORT_COLLADA_SETUP_H
|
||||
#define IMPORT_COLLADA_SETUP_H
|
||||
|
||||
class ExampleInterface* ImportColladaCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
|
||||
|
||||
|
||||
#endif //IMPORT_COLLADA_SETUP_H
|
||||
748
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
Normal file
748
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
Normal file
@@ -0,0 +1,748 @@
|
||||
/*
|
||||
Bullet Collision Detection and Physics Library http://bulletphysics.org
|
||||
This file is Copyright (c) 2014 Google Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
//original author: Erwin Coumans
|
||||
*/
|
||||
|
||||
|
||||
#include "LoadMeshFromCollada.h"
|
||||
#include <stdio.h> //fopen
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include <string>
|
||||
#include "tinyxml/tinyxml.h"
|
||||
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
#include "LinearMath/btHashMap.h"
|
||||
#include <assert.h>
|
||||
#include "btMatrix4x4.h"
|
||||
|
||||
|
||||
|
||||
|
||||
struct VertexSource
|
||||
{
|
||||
std::string m_positionArrayId;
|
||||
std::string m_normalArrayId;
|
||||
};
|
||||
|
||||
struct TokenFloatArray
|
||||
{
|
||||
btAlignedObjectArray<float>& m_values;
|
||||
TokenFloatArray(btAlignedObjectArray<float>& floatArray)
|
||||
:m_values(floatArray) {
|
||||
}
|
||||
inline void add(const char* token)
|
||||
{
|
||||
float v = atof(token);
|
||||
m_values.push_back(v);
|
||||
}
|
||||
};
|
||||
struct TokenIntArray
|
||||
{
|
||||
btAlignedObjectArray<int>& m_values;
|
||||
TokenIntArray(btAlignedObjectArray<int>& intArray)
|
||||
:m_values(intArray) {
|
||||
}
|
||||
inline void add(const char* token)
|
||||
{
|
||||
float v = atoi(token);
|
||||
m_values.push_back(v);
|
||||
}
|
||||
};
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void readFloatArray(TiXmlElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride)
|
||||
{
|
||||
int numVals, stride;
|
||||
TiXmlElement* array = source->FirstChildElement("float_array");
|
||||
if(array)
|
||||
{
|
||||
componentStride = 1;
|
||||
if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride)!= TIXML_NO_ATTRIBUTE)
|
||||
{
|
||||
componentStride = stride;
|
||||
}
|
||||
array->QueryIntAttribute("count", &numVals);
|
||||
TokenFloatArray adder(floatArray);
|
||||
floatArray.reserve(numVals);
|
||||
tokenize(array->GetText(),adder);
|
||||
assert(floatArray.size() == numVals);
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 getVector3FromXmlText(const char* text)
|
||||
{
|
||||
btVector3 vec(0,0,0);
|
||||
btAlignedObjectArray<float> floatArray;
|
||||
TokenFloatArray adder(floatArray);
|
||||
floatArray.reserve(3);
|
||||
tokenize(text,adder);
|
||||
assert(floatArray.size() == 3);
|
||||
if (floatArray.size()==3)
|
||||
{
|
||||
vec.setValue(floatArray[0],floatArray[1],floatArray[2]);
|
||||
}
|
||||
return vec;
|
||||
}
|
||||
|
||||
btVector4 getVector4FromXmlText(const char* text)
|
||||
{
|
||||
btVector4 vec(0,0,0,0);
|
||||
btAlignedObjectArray<float> floatArray;
|
||||
TokenFloatArray adder(floatArray);
|
||||
floatArray.reserve(4);
|
||||
tokenize(text,adder);
|
||||
assert(floatArray.size() == 4);
|
||||
if (floatArray.size()==4)
|
||||
{
|
||||
vec.setValue(floatArray[0],floatArray[1],floatArray[2],floatArray[3]);
|
||||
}
|
||||
return vec;
|
||||
}
|
||||
|
||||
|
||||
void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling)
|
||||
{
|
||||
btHashMap<btHashString,TiXmlElement* > allSources;
|
||||
btHashMap<btHashString,VertexSource> vertexSources;
|
||||
for(TiXmlElement* 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 (TiXmlElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
|
||||
{
|
||||
TiXmlElement* vertices2 = mesh->FirstChildElement("vertices");
|
||||
|
||||
for (TiXmlElement* 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* vertexId = vertices2->Attribute("id");
|
||||
//printf("vertices id=%s\n",vertexId);
|
||||
VertexSource vs;
|
||||
for(TiXmlElement* 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);
|
||||
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")
|
||||
{
|
||||
vs.m_positionArrayId = source_name;
|
||||
}
|
||||
if (semName=="NORMAL")
|
||||
{
|
||||
vs.m_normalArrayId = source_name;
|
||||
}
|
||||
}
|
||||
vertexSources.insert(vertexId,vs);
|
||||
|
||||
for (TiXmlElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles"))
|
||||
{
|
||||
std::string positionSourceName;
|
||||
std::string normalSourceName;
|
||||
int primitiveCount;
|
||||
primitive->QueryIntAttribute("count", &primitiveCount);
|
||||
bool positionAndNormalInVertex=false;
|
||||
int indexStride=1;
|
||||
int posOffset = 0;
|
||||
int normalOffset = 0;
|
||||
int numIndices = 0;
|
||||
{
|
||||
for (TiXmlElement* 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;
|
||||
//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=="VERTEX")
|
||||
{
|
||||
//now we have POSITION and possibly NORMAL too, using same index array (<p>)
|
||||
VertexSource* vs = vertexSources[source_name.c_str()];
|
||||
if (vs->m_positionArrayId.length())
|
||||
{
|
||||
positionSourceName = vs->m_positionArrayId;
|
||||
posOffset = offset;
|
||||
}
|
||||
if (vs->m_normalArrayId.length())
|
||||
{
|
||||
normalSourceName = vs->m_normalArrayId;
|
||||
normalOffset = offset;
|
||||
positionAndNormalInVertex = true;
|
||||
}
|
||||
}
|
||||
if (semName=="NORMAL")
|
||||
{
|
||||
btAssert(normalSourceName.length()==0);
|
||||
normalSourceName = source_name;
|
||||
normalOffset = offset;
|
||||
positionAndNormalInVertex = false;
|
||||
}
|
||||
}
|
||||
numIndices = primitiveCount * 3;
|
||||
}
|
||||
btAlignedObjectArray<float> positionFloatArray;
|
||||
int posStride=1;
|
||||
TiXmlElement** sourcePtr = allSources[positionSourceName.c_str()];
|
||||
if (sourcePtr)
|
||||
{
|
||||
readFloatArray(*sourcePtr,positionFloatArray, posStride);
|
||||
}
|
||||
btAlignedObjectArray<float> normalFloatArray;
|
||||
int normalStride=1;
|
||||
sourcePtr = allSources[normalSourceName.c_str()];
|
||||
if (sourcePtr)
|
||||
{
|
||||
readFloatArray(*sourcePtr,normalFloatArray,normalStride);
|
||||
}
|
||||
btAlignedObjectArray<int> curIndices;
|
||||
curIndices.reserve(numIndices*indexStride);
|
||||
TokenIntArray adder(curIndices);
|
||||
tokenize(primitive->FirstChildElement("p")->GetText(),adder);
|
||||
assert(curIndices.size() == numIndices*indexStride);
|
||||
int indexOffset = vertexPositions.size();
|
||||
|
||||
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))
|
||||
{
|
||||
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));
|
||||
}
|
||||
}
|
||||
int curNumIndices = indices.size();
|
||||
indices.resize(curNumIndices+numIndices);
|
||||
for(int index=0; index<numIndices; index++)
|
||||
{
|
||||
indices[curNumIndices+index] = index+indexOffset;
|
||||
}
|
||||
}//if(primitive != NULL)
|
||||
}//for each mesh
|
||||
|
||||
int shapeIndex = visualShapes.size();
|
||||
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);
|
||||
}
|
||||
|
||||
for (int index=0;index<indices.size();index++)
|
||||
{
|
||||
visualShape.m_indices->push_back(indices[index]+indexBase);
|
||||
}
|
||||
|
||||
|
||||
printf(" 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();
|
||||
}
|
||||
printf("geometry name=%s\n",geometryName);
|
||||
name2Shape.insert(geometryName,shapeIndex);
|
||||
|
||||
|
||||
}//for each geometry
|
||||
}
|
||||
|
||||
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
|
||||
{
|
||||
const char* nodeName = node->Attribute("id");
|
||||
printf("processing node %s\n", nodeName);
|
||||
|
||||
|
||||
btMatrix4x4 nodeTrans;
|
||||
nodeTrans.setIdentity();
|
||||
|
||||
///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
|
||||
{
|
||||
for (TiXmlElement* 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)
|
||||
{
|
||||
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
|
||||
{
|
||||
printf("Error: expected 16 elements in a <matrix> element, skipping\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
|
||||
{
|
||||
if (transElem->GetText())
|
||||
{
|
||||
btVector3 pos = getVector3FromXmlText(transElem->GetText());
|
||||
//nodePos+= unitScaling*parentScaling*pos;
|
||||
btMatrix4x4 t;
|
||||
t.setPureTranslation(pos);
|
||||
nodeTrans = nodeTrans*t;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
for(TiXmlElement* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
for(TiXmlElement* 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
|
||||
btMatrix4x4 t;
|
||||
t.setPureRotation(orn);
|
||||
nodeTrans = nodeTrans*t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nodeTrans = parentTransMat*nodeTrans;
|
||||
|
||||
for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry");
|
||||
instanceGeom!=0;
|
||||
instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
|
||||
{
|
||||
const char* geomUrl = instanceGeom->Attribute("url");
|
||||
printf("node referring to geom %s\n", geomUrl);
|
||||
geomUrl++;
|
||||
int* shapeIndexPtr = name2Shape[geomUrl];
|
||||
if (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
|
||||
{
|
||||
printf("geom not found\n");
|
||||
}
|
||||
}
|
||||
|
||||
for(TiXmlElement* childNode = node->FirstChildElement("node");
|
||||
childNode!= NULL; childNode = childNode->NextSiblingElement("node"))
|
||||
{
|
||||
readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans);
|
||||
}
|
||||
}
|
||||
void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
|
||||
{
|
||||
btHashMap<btHashString,TiXmlElement* > allVisualScenes;
|
||||
|
||||
TiXmlElement* libVisualScenes = doc.RootElement()->FirstChildElement("library_visual_scenes");
|
||||
if (libVisualScenes==0)
|
||||
return;
|
||||
|
||||
{
|
||||
for(TiXmlElement* scene = libVisualScenes->FirstChildElement("visual_scene");
|
||||
scene != NULL; scene = scene->NextSiblingElement("visual_scene"))
|
||||
{
|
||||
const char* sceneName = scene->Attribute("id");
|
||||
allVisualScenes.insert(sceneName,scene);
|
||||
}
|
||||
}
|
||||
|
||||
TiXmlElement* scene = 0;
|
||||
{
|
||||
TiXmlElement* scenes = doc.RootElement()->FirstChildElement("scene");
|
||||
if (scenes)
|
||||
{
|
||||
TiXmlElement* instanceSceneReference = scenes->FirstChildElement("instance_visual_scene");
|
||||
if (instanceSceneReference)
|
||||
{
|
||||
const char* instanceSceneUrl = instanceSceneReference->Attribute("url");
|
||||
TiXmlElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl+1];//skip #
|
||||
if (sceneInstancePtr)
|
||||
{
|
||||
scene = *sceneInstancePtr;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (scene)
|
||||
{
|
||||
for(TiXmlElement* node = scene->FirstChildElement("node");
|
||||
node != NULL; node = node->NextSiblingElement("node"))
|
||||
{
|
||||
btMatrix4x4 identity;
|
||||
identity.setIdentity();
|
||||
btVector3 identScaling(1,1,1);
|
||||
readNodeHierarchy(node,name2Shape,visualShapeInstances, identity);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
|
||||
{
|
||||
///todo(erwincoumans) those up-axis transformations have been quickly coded without rigorous testing
|
||||
|
||||
TiXmlElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
|
||||
if (unitMeter)
|
||||
{
|
||||
const char* meterText = unitMeter->Attribute("meter");
|
||||
printf("meterText=%s\n", meterText);
|
||||
unitMeterScaling = atof(meterText);
|
||||
}
|
||||
|
||||
TiXmlElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis");
|
||||
if (upAxisElem)
|
||||
{
|
||||
switch (clientUpAxis)
|
||||
{
|
||||
|
||||
case 1:
|
||||
{
|
||||
std::string upAxisTxt = upAxisElem->GetText();
|
||||
if (upAxisTxt == "X_UP")
|
||||
{
|
||||
btQuaternion x2y(btVector3(0,0,1),SIMD_HALF_PI);
|
||||
tr.setRotation(x2y);
|
||||
}
|
||||
if (upAxisTxt == "Y_UP")
|
||||
{
|
||||
//assume Y_UP for now, to be compatible with assimp?
|
||||
//client and COLLADA are both Z_UP so no transform needed (identity)
|
||||
}
|
||||
if (upAxisTxt == "Z_UP")
|
||||
{
|
||||
btQuaternion z2y(btVector3(1,0,0),-SIMD_HALF_PI);
|
||||
tr.setRotation(z2y);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
std::string upAxisTxt = upAxisElem->GetText();
|
||||
if (upAxisTxt == "X_UP")
|
||||
{
|
||||
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);
|
||||
tr.setRotation(y2z);
|
||||
}
|
||||
if (upAxisTxt == "Z_UP")
|
||||
{
|
||||
//client and COLLADA are both Z_UP so no transform needed (identity)
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 0:
|
||||
default:
|
||||
{
|
||||
//we don't support X or other up axis
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis)
|
||||
{
|
||||
|
||||
GLInstanceGraphicsShape* instance = 0;
|
||||
|
||||
//usually COLLADA files don't have that many visual geometries/shapes
|
||||
visualShapes.reserve(32);
|
||||
|
||||
float extraScaling = 1;//0.01;
|
||||
btHashMap<btHashString, int> name2ShapeIndex;
|
||||
b3FileUtils f;
|
||||
char filename[1024];
|
||||
if (!f.findFile(relativeFileName,filename,1024))
|
||||
{
|
||||
printf("File not found: %s\n", filename);
|
||||
return;
|
||||
}
|
||||
|
||||
TiXmlDocument doc(filename);
|
||||
if (!doc.LoadFile())
|
||||
return;
|
||||
|
||||
//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);
|
||||
|
||||
btMatrix4x4 ident;
|
||||
ident.setIdentity();
|
||||
|
||||
readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
|
||||
|
||||
readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#ifdef COMPARE_WITH_ASSIMP
|
||||
|
||||
#include <assimp/Importer.hpp>
|
||||
#include <assimp/mesh.h>
|
||||
#include <assimp/postprocess.h>
|
||||
#include <assimp/scene.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"
|
||||
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
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++)
|
||||
{
|
||||
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);
|
||||
}
|
||||
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)
|
||||
{
|
||||
upAxisTrans.setIdentity();
|
||||
unitMeterScaling=1;
|
||||
|
||||
GLInstanceGraphicsShape* shape = 0;
|
||||
|
||||
|
||||
FILE* file = fopen(relativeFileName,"rb");
|
||||
if (file)
|
||||
{
|
||||
int size=0;
|
||||
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
|
||||
{
|
||||
printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
|
||||
} 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);
|
||||
aiScene const* scene = importer.ReadFile(relativeFileName,
|
||||
aiProcess_JoinIdenticalVertices |
|
||||
//aiProcess_RemoveComponent |
|
||||
aiProcess_SortByPType |
|
||||
aiProcess_Triangulate);
|
||||
if (scene)
|
||||
{
|
||||
shape = &visualShapes.expand();
|
||||
shape->m_scaling[0] = 1;
|
||||
shape->m_scaling[1] = 1;
|
||||
shape->m_scaling[2] = 1;
|
||||
shape->m_scaling[3] = 1;
|
||||
int index = 0;
|
||||
shape->m_indices = new b3AlignedObjectArray<int>();
|
||||
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
aiMatrix4x4 ident;
|
||||
addMeshParts(scene, scene->mRootNode, shape, ident);
|
||||
shape->m_numIndices = shape->m_indices->size();
|
||||
shape->m_numvertices = shape->m_vertices->size();
|
||||
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
|
||||
instance.m_shapeIndex = visualShapes.size()-1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif //COMPARE_WITH_ASSIMP
|
||||
45
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
Normal file
45
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
Normal file
@@ -0,0 +1,45 @@
|
||||
/*
|
||||
Bullet Collision Detection and Physics Library http://bulletphysics.org
|
||||
This file is Copyright (c) 2014 Google Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
//original author: Erwin Coumans
|
||||
*/
|
||||
|
||||
|
||||
#ifndef LOAD_MESH_FROM_COLLADA_H
|
||||
#define LOAD_MESH_FROM_COLLADA_H
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include "ColladaGraphicsInstance.h"
|
||||
|
||||
|
||||
void LoadMeshFromCollada(const char* relativeFileName,
|
||||
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
|
||||
|
||||
#endif //LOAD_MESH_FROM_COLLADA_H
|
||||
156
examples/Importers/ImportColladaDemo/btMatrix4x4.h
Normal file
156
examples/Importers/ImportColladaDemo/btMatrix4x4.h
Normal file
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
Bullet Collision Detection and Physics Library http://bulletphysics.org
|
||||
This file is Copyright (c) 2014 Google Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
//original author: Erwin Coumans
|
||||
*/
|
||||
|
||||
|
||||
#ifndef MATRIX4x4_H
|
||||
#define MATRIX4x4_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#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
|
||||
{
|
||||
btVector4 m_el[4];
|
||||
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)
|
||||
{
|
||||
setValue(xx, xy, xz, xw,
|
||||
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 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);
|
||||
}
|
||||
inline void setPureRotation(const btQuaternion& orn)
|
||||
{
|
||||
setIdentity();
|
||||
|
||||
btMatrix3x3 m3(orn);
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
for (int j=0;j<3;j++)
|
||||
{
|
||||
m_el[i][j] = m3[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
SIMD_FORCE_INLINE const btVector4& operator[](int i) const
|
||||
{
|
||||
btFullAssert(0 <= i && i < 3);
|
||||
return m_el[i];
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
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
|
||||
{
|
||||
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
|
||||
{
|
||||
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
|
||||
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]));
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
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();
|
||||
}
|
||||
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));
|
||||
}
|
||||
|
||||
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
|
||||
105
examples/Importers/ImportObjDemo/ImportObjExample.cpp
Normal file
105
examples/Importers/ImportObjDemo/ImportObjExample.cpp
Normal file
@@ -0,0 +1,105 @@
|
||||
#include "ImportObjExample.h"
|
||||
#include <vector>
|
||||
#include "../OpenGLWindow/GLInstancingRenderer.h"
|
||||
#include"../Wavefront/tiny_obj_loader.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "../OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "Wavefront2GLInstanceGraphicsShape.h"
|
||||
|
||||
|
||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||
|
||||
|
||||
class ImportObjSetup : public CommonRigidBodyBase
|
||||
{
|
||||
|
||||
public:
|
||||
ImportObjSetup(struct GUIHelperInterface* helper);
|
||||
virtual ~ImportObjSetup();
|
||||
|
||||
virtual void initPhysics();
|
||||
};
|
||||
|
||||
ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
ImportObjSetup::~ImportObjSetup()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void ImportObjSetup::initPhysics()
|
||||
{
|
||||
m_guiHelper->setUpAxis(2);
|
||||
this->createEmptyDynamicsWorld();
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
|
||||
const char* fileName = "samurai_monastry.obj";
|
||||
char relativeFileName[1024];
|
||||
const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"};
|
||||
int prefixIndex=-1;
|
||||
{
|
||||
|
||||
int numPrefixes = sizeof(prefix)/sizeof(char*);
|
||||
|
||||
for (int i=0;i<numPrefixes;i++)
|
||||
{
|
||||
FILE* f = 0;
|
||||
sprintf(relativeFileName,"%s%s",prefix[i],fileName);
|
||||
f = fopen(relativeFileName,"r");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
prefixIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (prefixIndex<0)
|
||||
return;
|
||||
|
||||
btVector3 shift(0,0,0);
|
||||
btVector3 scaling(10,10,10);
|
||||
// int index=10;
|
||||
|
||||
{
|
||||
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, relativeFileName, prefix[prefixIndex]);
|
||||
|
||||
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
|
||||
|
||||
btTransform trans;
|
||||
trans.setIdentity();
|
||||
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);
|
||||
|
||||
//int id =
|
||||
m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
struct ExampleInterface* ImportObjCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
|
||||
{
|
||||
return new ImportObjSetup(helper);
|
||||
}
|
||||
7
examples/Importers/ImportObjDemo/ImportObjExample.h
Normal file
7
examples/Importers/ImportObjDemo/ImportObjExample.h
Normal file
@@ -0,0 +1,7 @@
|
||||
#ifndef IMPORT_OBJ_EXAMPLE_H
|
||||
#define IMPORT_OBJ_EXAMPLE_H
|
||||
|
||||
struct ExampleInterface* ImportObjCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
|
||||
|
||||
|
||||
#endif //IMPORT_OBJ_EXAMPLE_H
|
||||
17
examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
Normal file
17
examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
Normal file
@@ -0,0 +1,17 @@
|
||||
#include "LoadMeshFromObj.h"
|
||||
#include"../Wavefront/tiny_obj_loader.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include <stdio.h> //fopen
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "Wavefront2GLInstanceGraphicsShape.h"
|
||||
|
||||
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath)
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, relativeFileName, materialPrefixPath);
|
||||
|
||||
GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
|
||||
return gfxShape;
|
||||
}
|
||||
11
examples/Importers/ImportObjDemo/LoadMeshFromObj.h
Normal file
11
examples/Importers/ImportObjDemo/LoadMeshFromObj.h
Normal file
@@ -0,0 +1,11 @@
|
||||
#ifndef LOAD_MESH_FROM_OBJ_H
|
||||
#define LOAD_MESH_FROM_OBJ_H
|
||||
|
||||
|
||||
struct GLInstanceGraphicsShape;
|
||||
|
||||
|
||||
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath);
|
||||
|
||||
#endif //LOAD_MESH_FROM_OBJ_H
|
||||
|
||||
@@ -0,0 +1,109 @@
|
||||
#include "Wavefront2GLInstanceGraphicsShape.h"
|
||||
|
||||
#include "../OpenGLWindow/GLInstancingRenderer.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "../OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "Wavefront2GLInstanceGraphicsShape.h"
|
||||
#include "../OpenGLWindow/GLInstancingRenderer.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
|
||||
GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes)
|
||||
{
|
||||
|
||||
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++)
|
||||
{
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
|
||||
|
||||
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);
|
||||
int vtxBaseIndex = vertices->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;
|
||||
|
||||
vtx0.uv[0] = 0.5f;//shape.mesh.positions[shape.mesh.indices[f]*3+2];?
|
||||
vtx0.uv[1] = 0.5f;
|
||||
|
||||
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.uv[0] = 0.5f;//obj->textureList[face->vertex_index[1]]->e[0];
|
||||
vtx1.uv[1] = 0.5f;//obj->textureList[face->vertex_index[1]]->e[1];
|
||||
|
||||
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[3] = 0.f;
|
||||
vtx2.uv[0] = 0.5f;
|
||||
vtx2.uv[1] = 0.5f;
|
||||
|
||||
|
||||
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]);
|
||||
|
||||
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];
|
||||
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
|
||||
return gfxShape;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
#ifndef WAVEFRONT2GRAPHICS_H
|
||||
#define WAVEFRONT2GRAPHICS_H
|
||||
|
||||
#include"../Wavefront/tiny_obj_loader.h"
|
||||
#include <vector>
|
||||
|
||||
struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes);
|
||||
|
||||
#endif //WAVEFRONT2GRAPHICS_H
|
||||
98
examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
Normal file
98
examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
Normal file
@@ -0,0 +1,98 @@
|
||||
#include "ImportSTLSetup.h"
|
||||
#include <vector>
|
||||
#include "../OpenGLWindow/GLInstancingRenderer.h"
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "../OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "LoadMeshFromSTL.h"
|
||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||
|
||||
|
||||
|
||||
|
||||
class ImportSTLSetup : public CommonRigidBodyBase
|
||||
{
|
||||
|
||||
public:
|
||||
ImportSTLSetup(struct GUIHelperInterface* helper);
|
||||
virtual ~ImportSTLSetup();
|
||||
|
||||
virtual void initPhysics();
|
||||
};
|
||||
|
||||
|
||||
ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
ImportSTLSetup::~ImportSTLSetup()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ImportSTLSetup::initPhysics()
|
||||
{
|
||||
m_guiHelper->setUpAxis(2);
|
||||
this->createEmptyDynamicsWorld();
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
|
||||
|
||||
const char* fileName = "l_finger_tip.stl";
|
||||
char relativeFileName[1024];
|
||||
const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"};
|
||||
int prefixIndex=-1;
|
||||
{
|
||||
|
||||
int numPrefixes = sizeof(prefix)/sizeof(char*);
|
||||
|
||||
for (int i=0;i<numPrefixes;i++)
|
||||
{
|
||||
FILE* f = 0;
|
||||
sprintf(relativeFileName,"%s%s",prefix[i],fileName);
|
||||
f = fopen(relativeFileName,"r");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
prefixIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (prefixIndex<0)
|
||||
return;
|
||||
|
||||
btVector3 shift(0,0,0);
|
||||
btVector3 scaling(10,10,10);
|
||||
// int index=10;
|
||||
|
||||
{
|
||||
|
||||
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
|
||||
|
||||
btTransform trans;
|
||||
trans.setIdentity();
|
||||
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,scaling);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
class ExampleInterface* ImportSTLCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
|
||||
{
|
||||
return new ImportSTLSetup(helper);
|
||||
}
|
||||
6
examples/Importers/ImportSTLDemo/ImportSTLSetup.h
Normal file
6
examples/Importers/ImportSTLDemo/ImportSTLSetup.h
Normal file
@@ -0,0 +1,6 @@
|
||||
#ifndef IMPORT_STL_SETUP_H
|
||||
#define IMPORT_STL_SETUP_H
|
||||
|
||||
class ExampleInterface* ImportSTLCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
|
||||
|
||||
#endif //IMPORT_OBJ_SETUP_H
|
||||
109
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
Normal file
109
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
Normal file
@@ -0,0 +1,109 @@
|
||||
|
||||
#ifndef LOAD_MESH_FROM_STL_H
|
||||
#define LOAD_MESH_FROM_STL_H
|
||||
|
||||
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
|
||||
#include <stdio.h> //fopen
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
struct MySTLTriangle
|
||||
{
|
||||
float normal[3];
|
||||
float vertex0[3];
|
||||
float vertex1[3];
|
||||
float vertex2[3];
|
||||
};
|
||||
|
||||
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
|
||||
{
|
||||
GLInstanceGraphicsShape* shape = 0;
|
||||
|
||||
FILE* file = fopen(relativeFileName,"rb");
|
||||
if (file)
|
||||
{
|
||||
int size=0;
|
||||
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
|
||||
{
|
||||
printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
|
||||
} else
|
||||
{
|
||||
if (size)
|
||||
{
|
||||
printf("Open STL file of %d bytes\n",size);
|
||||
char* memoryBuffer = new char[size+1];
|
||||
int actualBytesRead = fread(memoryBuffer,1,size,file);
|
||||
if (actualBytesRead!=size)
|
||||
{
|
||||
printf("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;
|
||||
if (expectedBinaryFileSize != size)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
shape = new GLInstanceGraphicsShape;
|
||||
// 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;
|
||||
shape->m_scaling[3] = 1;
|
||||
int index = 0;
|
||||
shape->m_indices = new b3AlignedObjectArray<int>();
|
||||
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
for (int i=0;i<numTriangles;i++)
|
||||
{
|
||||
char* curPtr = &memoryBuffer[84+i*50];
|
||||
MySTLTriangle* tri = (MySTLTriangle*) curPtr;
|
||||
|
||||
GLInstanceVertex v0,v1,v2;
|
||||
if (i==numTriangles-2)
|
||||
{
|
||||
printf("!\n");
|
||||
}
|
||||
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
|
||||
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
|
||||
for (int v=0;v<3;v++)
|
||||
{
|
||||
v0.xyzw[v] = tri->vertex0[v];
|
||||
v1.xyzw[v] = tri->vertex1[v];
|
||||
v2.xyzw[v] = tri->vertex2[v];
|
||||
v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
|
||||
}
|
||||
v0.xyzw[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;
|
||||
}
|
||||
}
|
||||
fclose(file);
|
||||
}
|
||||
shape->m_numIndices = shape->m_indices->size();
|
||||
shape->m_numvertices = shape->m_vertices->size();
|
||||
return shape;
|
||||
}
|
||||
|
||||
#endif //LOAD_MESH_FROM_STL_H
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef CONVERT_RIGIDBODIES_2_MULTIBODY_H
|
||||
#define CONVERT_RIGIDBODIES_2_MULTIBODY_H
|
||||
|
||||
struct ConvertRigidBodies2MultiBody
|
||||
{
|
||||
btAlignedObjectArray<btRigidBody*> m_bodies;
|
||||
btAlignedObjectArray<btTypedConstraint*> m_constraints;
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body);
|
||||
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
|
||||
virtual btMultiBody* convertToMultiBody();
|
||||
|
||||
};
|
||||
#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H
|
||||
1702
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
Normal file
1702
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
Normal file
File diff suppressed because it is too large
Load Diff
7
examples/Importers/ImportURDFDemo/ImportURDFSetup.h
Normal file
7
examples/Importers/ImportURDFDemo/ImportURDFSetup.h
Normal file
@@ -0,0 +1,7 @@
|
||||
#ifndef IMPORT_URDF_SETUP_H
|
||||
#define IMPORT_URDF_SETUP_H
|
||||
|
||||
|
||||
class ExampleInterface* ImportURDFCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
|
||||
|
||||
#endif //IMPORT_URDF_SETUP_H
|
||||
507
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
Normal file
507
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
Normal file
@@ -0,0 +1,507 @@
|
||||
#include "URDF2Bullet.h"
|
||||
#include <stdio.h>
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
|
||||
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
|
||||
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
|
||||
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),
|
||||
};
|
||||
|
||||
|
||||
static btVector3 selectColor2()
|
||||
{
|
||||
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
return color;
|
||||
}
|
||||
|
||||
void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
|
||||
int numChildren = childIndices.size();
|
||||
|
||||
indentationLevel+=2;
|
||||
int count = 0;
|
||||
for (int i=0;i<numChildren;i++)
|
||||
{
|
||||
int childLinkIndex = childIndices[i];
|
||||
std::string name = u2b.getLinkName(childLinkIndex);
|
||||
for(int j=0;j<indentationLevel;j++) printf(" "); //indent
|
||||
printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
|
||||
// first grandchild
|
||||
printTree(u2b,childLinkIndex,indentationLevel);
|
||||
}
|
||||
}
|
||||
|
||||
struct URDF2BulletCachedData
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
:m_totalNumJoints1(0),
|
||||
m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0)
|
||||
{
|
||||
|
||||
}
|
||||
//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;
|
||||
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
|
||||
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* 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 btCompoundShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
|
||||
for (int i=0;i<childIndices.size();i++)
|
||||
{
|
||||
printf("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 URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
|
||||
{
|
||||
//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;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
{
|
||||
printf("start converting/extracting data from URDF interface\n");
|
||||
|
||||
btTransform linkTransformInWorldSpace;
|
||||
linkTransformInWorldSpace.setIdentity();
|
||||
|
||||
|
||||
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
|
||||
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
||||
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
||||
btRigidBody* parentRigidBody = 0;
|
||||
|
||||
std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
|
||||
printf("mb link index = %d\n",mbLinkIndex);
|
||||
|
||||
btTransform parentLocalInertialFrame;
|
||||
parentLocalInertialFrame.setIdentity();
|
||||
btScalar parentMass(1);
|
||||
btVector3 parentLocalInertiaDiagonal(1,1,1);
|
||||
|
||||
if (urdfParentIndex==-2)
|
||||
{
|
||||
printf("root link has no parent\n");
|
||||
} else
|
||||
{
|
||||
printf("urdf parent index = %d\n",urdfParentIndex);
|
||||
printf("mb parent index = %d\n",mbParentIndex);
|
||||
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
||||
u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
|
||||
|
||||
}
|
||||
|
||||
btScalar mass = 0;
|
||||
btTransform localInertialFrame;
|
||||
localInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0,0,0);
|
||||
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btTransform parent2joint;
|
||||
parent2joint.setIdentity();
|
||||
|
||||
int jointType;
|
||||
btVector3 jointAxisInJointSpace;
|
||||
btScalar jointLowerLimit;
|
||||
btScalar jointUpperLimit;
|
||||
|
||||
|
||||
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
|
||||
|
||||
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
|
||||
|
||||
int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
|
||||
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
|
||||
if (compoundShape)
|
||||
{
|
||||
|
||||
|
||||
btVector3 color = selectColor2();
|
||||
/*
|
||||
if (visual->material.get())
|
||||
{
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
}
|
||||
*/
|
||||
//btVector3 localInertiaDiagonal(0, 0, 0);
|
||||
//if (mass)
|
||||
//{
|
||||
// shape->calculateLocalInertia(mass, localInertiaDiagonal);
|
||||
//}
|
||||
|
||||
btRigidBody* linkRigidBody = 0;
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
linkRigidBody = body;
|
||||
|
||||
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
} else
|
||||
{
|
||||
if (cache.m_bulletMultiBody==0)
|
||||
{
|
||||
bool multiDof = true;
|
||||
bool canSleep = false;
|
||||
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint) {
|
||||
|
||||
btTransform offsetInA,offsetInB;
|
||||
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
|
||||
bool disableParentCollision = true;
|
||||
switch (jointType)
|
||||
{
|
||||
case URDF2Bullet::FixedJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
|
||||
printf("Fixed joint (btMultiBody)\n");
|
||||
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
|
||||
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
btMatrix3x3 rm(rot);
|
||||
btScalar y,p,r;
|
||||
rm.getEulerZYX(y,p,r);
|
||||
printf("y=%f,p=%f,r=%f\n", y,p,r);
|
||||
|
||||
} else
|
||||
{
|
||||
printf("Fixed joint\n");
|
||||
|
||||
btMatrix3x3 rm(offsetInA.getBasis());
|
||||
btScalar y,p,r;
|
||||
rm.getEulerZYX(y,p,r);
|
||||
printf("y=%f,p=%f,r=%f\n", y,p,r);
|
||||
|
||||
//we could also use btFixedConstraint but it has some issues
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,0));
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDF2Bullet::ContinuousJoint:
|
||||
case URDF2Bullet::RevoluteJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
|
||||
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
} else
|
||||
{
|
||||
//only handle principle axis at the moment,
|
||||
//@todo(erwincoumans) orient the constraint for non-principal axis
|
||||
int principleAxis = jointAxisInJointSpace.closestAxis();
|
||||
switch (principleAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
dof6->setAngularUpperLimit(btVector3(-1,0,0));
|
||||
dof6->setAngularLowerLimit(btVector3(1,0,0));
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
dof6->setAngularUpperLimit(btVector3(0,-1,0));
|
||||
dof6->setAngularLowerLimit(btVector3(0,1,0));
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,-1));
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
}
|
||||
};
|
||||
printf("Revolute/Continuous joint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDF2Bullet::PrismaticJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
} else
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
|
||||
int principleAxis = jointAxisInJointSpace.closestAxis();
|
||||
switch (principleAxis)
|
||||
{
|
||||
case 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));
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
|
||||
}
|
||||
};
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,0));
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
|
||||
printf("Prismatic\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Error: unsupported joint type in URDF (%d)\n", jointType);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
col->setCollisionShape(compoundShape);
|
||||
|
||||
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);
|
||||
|
||||
bool isDynamic = true;
|
||||
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
|
||||
|
||||
btVector3 color = selectColor2();//(0.0,0.0,0.5);
|
||||
|
||||
u2b.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
|
||||
|
||||
btScalar friction = 0.5f;
|
||||
|
||||
col->setFriction(friction);
|
||||
|
||||
if (mbLinkIndex>=0) //???? double-check +/- 1
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
|
||||
} else
|
||||
{
|
||||
cache.m_bulletMultiBody->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btAlignedObjectArray<int> urdfChildIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
|
||||
|
||||
int numChildren = urdfChildIndices.size();
|
||||
|
||||
for (int i=0;i<numChildren;i++)
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
{
|
||||
URDF2BulletCachedData cache;
|
||||
|
||||
InitURDF2BulletCache(u2b,cache);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
ConvertURDF2BulletInternal(u2b, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
|
||||
}
|
||||
|
||||
|
||||
71
examples/Importers/ImportURDFDemo/URDF2Bullet.h
Normal file
71
examples/Importers/ImportURDFDemo/URDF2Bullet.h
Normal file
@@ -0,0 +1,71 @@
|
||||
#ifndef _URDF2BULLET_H
|
||||
#define _URDF2BULLET_H
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include <string>
|
||||
class btVector3;
|
||||
class btTransform;
|
||||
class btMultiBodyDynamicsWorld;
|
||||
class btTransform;
|
||||
|
||||
class URDF2Bullet
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
enum {
|
||||
RevoluteJoint=1,
|
||||
PrismaticJoint,
|
||||
ContinuousJoint,
|
||||
FloatingJoint,
|
||||
PlanarJoint,
|
||||
FixedJoint,
|
||||
};
|
||||
|
||||
///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 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;
|
||||
|
||||
///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, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
|
||||
|
||||
///create Bullet collision shapes from URDF 'Collision' objects, specified in inertial frame of the link.
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0;
|
||||
|
||||
///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) const = 0;
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const =0;
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) const = 0;
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) const = 0;
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const = 0;
|
||||
};
|
||||
|
||||
|
||||
void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0);
|
||||
|
||||
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix);
|
||||
|
||||
|
||||
#endif //_URDF2BULLET_H
|
||||
|
||||
825
examples/Importers/ImportURDFDemo/urdf_samples.h
Normal file
825
examples/Importers/ImportURDFDemo/urdf_samples.h
Normal file
@@ -0,0 +1,825 @@
|
||||
#ifndef URDF_SAMPLES_H
|
||||
#define URDF_SAMPLES_H
|
||||
|
||||
#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>);
|
||||
|
||||
const char* urdf_char1 = MSTRINGIFY(
|
||||
<?xml version="1.0"?>
|
||||
<robot name="myfirst">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
);
|
||||
|
||||
const char* urdf_char3 = MSTRINGIFY(<?xml version="1.0"?>
|
||||
<robot name="multipleshapes">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
</joint>
|
||||
|
||||
</robot>);
|
||||
|
||||
const char* urdf_char4 = MSTRINGIFY(
|
||||
|
||||
<?xml version="1.0"?>
|
||||
<robot name="materials">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 0.1414 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
);
|
||||
|
||||
const char* urdf_char_r2d2 = MSTRINGIFY(
|
||||
|
||||
<?xml version="1.0"?>
|
||||
<robot name="visual">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="fixed">
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="fixed">
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="fixed">
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="fixed">
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
<material name="Gray">
|
||||
<color rgba=".7 .7 .7 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 0.1414 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
);
|
||||
|
||||
const char* urdf_char = MSTRINGIFY(
|
||||
|
||||
<?xml version="1.0"?>
|
||||
<robot name="physics">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 .2 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
<material name="Gray">
|
||||
<color rgba=".7 .7 .7 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="l_finger.stl"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_tip.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_tip.stl"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="l_finger.stl"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_tip.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_tip.stl"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".08 .08 .08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0 0.1414 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
);
|
||||
|
||||
#endif //URDF_SAMPLES_H
|
||||
|
||||
Reference in New Issue
Block a user